A time-varying path planning method

Abstract

The application discloses a time-varying path planning method, comprising the following steps: S1, determining path initial data of a mechanical arm, including a starting point x start and a termination point x end of the path; S2, acquiring feature information of an obstacle at a current time; S3, judging whether obstacle avoidance is needed; when obstacle avoidance is not needed, generating a path, and ending path planning; when obstacle avoidance is needed, obtaining a center point O0 of the path without obstacle avoidance in an obstacle space, and entering step S4; S4, determining a fuzzy center C of the obstacle according to feature points of the obstacle, and determining a direction of the path bypassing the obstacle according to the fuzzy center C of the obstacle and the path initial data of the mechanical arm; S5, determining feature points of the path according to the center point O0 and the direction of the path bypassing the obstacle; S6, generating the path at the current time according to the feature points of the path; and S7, returning to step S2 at a next time. The application can effectively improve path planning efficiency, adjust a smooth path in real time, and is particularly suitable for global dynamic obstacle avoidance of moving objects such as a snake-shaped mechanical arm.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm path planning technology, specifically relating to a time-varying path planning method. Background Technology

[0002] With the development of the times, more and more equipment is being used to replace human labor. Scholars both domestically and internationally have conducted extensive research on path planning for robotic arms, robots, cars, and remote-controlled aircraft. Snake-like robotic arms possess great flexibility, capable of performing not only basic tasks but also additional tasks such as obstacle avoidance. However, current research on real-time path planning related to snake-like robotic arms avoiding dynamic obstacles is limited.

[0003] In existing technologies, Bézier curves are generally used to plan the path of a single-point moving object, similar to a car. This type of path planning method is performed in a static obstacle environment and cannot achieve obstacle avoidance for complex moving objects in a dynamic environment. Therefore, there is a need to propose a novel planning method that can plan time-varying paths, enabling a serpentine robotic arm to achieve obstacle avoidance when moving along the time-varying repeating path. Summary of the Invention

[0004] The purpose of this invention is to overcome the above-mentioned defects and provide a time-varying path planning method. This method solves the technical problems of low efficiency and inability to be applied to global obstacle avoidance in traditional path planning methods. Furthermore, it solves the technical problem of non-smooth planned paths obtained by traditional path planning methods. This invention can effectively improve path planning efficiency and adjust smooth paths in real time. It is particularly suitable for global dynamic obstacle avoidance of moving objects such as snake-shaped robotic arms.

[0005] To achieve the above-mentioned objectives, the present invention provides the following technical solution:

[0006] This invention discloses a time-varying path planning method. For obstacles of different locations and shapes, it extracts a finite number of obstacle feature points to represent obstacle information, and determines the obstacle center point O0 and the fuzzy centroid point. Based on the preceding steps, feature points of the path curve are obtained, and then the corresponding path curve for the current moment, such as a Bézier spline curve, is generated based on these feature points. The path curve obtained at this moment serves as the motion path for that moment, allowing the moving object to move along this path. In dynamic single-obstacle or multi-obstacle environments, this invention plans a path using a path planning algorithm, and then adjusts the path in real time based on the nodes on the path and the feature points of dynamic obstacles, achieving global dynamic obstacle avoidance.

[0007] A time-varying path planning method, comprising:

[0008] S1 determines the initial path data for the robotic arm based on the task requirements; the initial path data includes the starting point x of the path. start and termination point xend ;

[0009] S2 acquires the feature information of obstacles at the current moment; the feature information of obstacles includes the shape data, position data, or size data of obstacles;

[0010] S3 determines whether obstacle avoidance is needed based on the initial path data of the robotic arm and the characteristic information of the obstacle;

[0011] When obstacle avoidance is not required, generate a path and end path planning;

[0012] When obstacle avoidance is required, the center point O0 of the part of the path that does not need to avoid obstacles within the obstacle space is obtained based on the initial path data of the robotic arm and the feature information of the obstacle, and then the process proceeds to step S4.

[0013] S4 acquires the feature points of the obstacle, determines the fuzzy center C of the obstacle based on the feature points, and determines the direction of the path around the obstacle based on the fuzzy center C of the obstacle and the initial path data of the robotic arm.

[0014] S5 determines the feature points of the path based on the center point O0 and the direction in which the path bypasses the obstacle;

[0015] S6 generates the current path based on the feature points of the path;

[0016] In the next moment, S7 returns to step S2.

[0017] Furthermore, in step S1, the initial data also includes the starting point direction d. start Or the direction of the termination point d end .

[0018] Furthermore, in step S3, the method for determining whether obstacle avoidance is needed based on the initial path data of the robotic arm and the characteristic information of the obstacle includes:

[0019] Connect the starting point x start With the termination point x end Generate a straight line, and determine whether the line intersects with the obstacle based on the obstacle's feature information. If they do not intersect, it is determined that no obstacle avoidance is required; if they intersect, it is determined that obstacle avoidance is required.

[0020] When obstacle avoidance is not required, use the straight line as the generated path and end the path planning.

[0021] When obstacle avoidance is required, the midpoint of the line segment where the straight line intersects the obstacle is taken as the center point O0.

[0022] Furthermore, the method in step S3 includes:

[0023] S3.1 Set the left moving point x left_temp The initial position is xstart ;

[0024] S3.2 Based on the feature information of the obstacle, determine x left_temp Whether it falls into the obstacle space; if it falls into the obstacle space, then x left_temp x is the first endpoint of the line segment where the straight line intersects the obstacle. left Proceed to step S3.4. If the obstacle is not within the obstacle space, proceed to step S3.3.

[0025] S3.3 Let x left_temp To x end stride length δ;

[0026] Determine x left_temp Has arrived or exceeded x? end If arriving at or exceeding x end The output will be in the form of x start and x end For a straight-line path between two endpoints, terminate path planning if the path is not reached or exceeds x. end Then return to step S3.2;

[0027] S3.4 Set the right moving point x right_temp The initial position is x end ;

[0028] S3.5 Based on the feature information of the obstacle, determine x right_temp Whether it falls into the obstacle space; if it falls into the obstacle space, then x right_temp The second endpoint x of the line segment where the straight line intersects the obstacle right Proceed to step S3.7. If the obstacle is not within the obstacle space, proceed to step S3.6.

[0029] S3.6 Let x right_temp To x start The movement step length δ is then returned to step S3.5;

[0030] S3.7 will be in x left and x right The midpoint of the line segment with two endpoints is taken as the center point O0.

[0031] Furthermore, in step S4, the feature points of the obstacle include the inflection points of the obstacle;

[0032] The coordinates of the fuzzy center C of the obstacle are the average of the coordinates of the feature points of each obstacle.

[0033] Furthermore, in step S4, when the two-dimensional obstacle is a shape without corners, the feature points of the obstacle are determined according to the following method:

[0034] Connect the starting point xstart With the termination point x end Generate the first straight line;

[0035] Draw lines 2 through N through the center point O0. Among the N lines, the angle between two adjacent lines is θ, where θ = 180° / (N-1); N ≥ 3.

[0036] The intersections of N straight lines with the boundary of the obstacle are taken as the feature points of the obstacle;

[0037] When a two-dimensional obstacle is a polygon, the feature points of the obstacle include the vertices of the polygon.

[0038] If an obstacle is an irregular shape, it can be equivalent to a regular shape such as a regular polygon or circle that contains the obstacle, that is, an irregular shape can be equivalent to a regular shape.

[0039] For 3D obstacles, the above method can also be used to determine obstacle feature points. For obstacles without inflection points, several evenly distributed straight lines can be drawn through the center point O0, and the intersections of these lines with the obstacle boundary can be used as obstacle feature points. For obstacles with inflection points, the inflection points can be used as obstacle feature points, or the intersections of the inflection points and the straight lines with the obstacle boundary can be used together as obstacle feature points. Points whose coordinate values ​​suddenly change beyond a predetermined threshold can be used as inflection points. The predetermined threshold can be set according to actual task requirements and accuracy.

[0040] Furthermore, methods for determining the direction of a path around an obstacle based on the fuzzy center C of the obstacle include:

[0041] Connect the starting point x start With the termination point x end Generate a straight line;

[0042] If the fuzzy center C of the obstacle is on one side of the straight line, the path will bypass the obstacle from the other side of the straight line, and the feature points of the obstacle located on that side will be recorded as the valid feature points of the obstacle.

[0043] If the fuzzy center C of the obstacle falls on the straight line, the path will bypass the obstacle from either side of the straight line.

[0044] Furthermore, let P be the feature point of the path. i 0 , i = 1, 2, ..., n;

[0045] In step S5, the method for determining the feature points of the path based on the center point O0 and the direction in which the path bypasses the obstacle includes:

[0046] S5.1 will x start and x end As P1 respectively 0 and

[0047] S5.2 Enlarge the boundary of the obstacle with the center point O0 as the center point, and denote the enlarged boundary and the set of points inside it as set K;

[0048] S5.3 over x start Draw a straight line along a first specific direction from the point, and extend the line until it intersects the set K. Take this intersection point as the set K. Pass x end Draw a straight line along the second specific direction from the point, and extend the line until it intersects the set K. Take this intersection point as the reference point.

[0049] When there is a starting point and direction d start Or the direction of the termination point d end When constrained, the first specific direction and the second specific direction are d respectively. start and d end ;

[0050] When the starting point direction d is not set start Or the direction of the termination point d end When constrained, both the first and second specific directions pass through x. start Point and x end The direction of the line connecting the points;

[0051] S5.4 The points in set K corresponding to the valid feature points of the obstacles are taken as...

[0052] Alternatively, establish a system with center point O0 as the origin, x start Point and x end The line connecting the points points points points along the x-axis. The +y-axis is perpendicular to the x-axis coordinate system and points in the direction in which the path bypasses the obstacle. Let n = 6.

[0053]

[0054] Wherein, min(K,x) is the minimum value of all points in set K along the x-axis, max(K,y) is the maximum value of all points in set K along the y-axis, and max(K,x) is the maximum value of all points in set K along the x-axis.

[0055] Furthermore, in step S6, the path curve generated at the current moment is a circular arc, a sine or cosine curve, an Nth-degree polynomial curve, a Bézier spline curve, or a B-spline curve.

[0056] In step S6, when the generated path curve is an arc, a sine or cosine curve, an Nth-order polynomial curve, or a B-spline curve, the path curve is a smooth curve passing through the feature points of each path.

[0057] When the generated path curve is a Bézier spline curve, an (n-1)th order Bézier curve is generated based on the characteristic points of the path, and the Bézier spline curve is determined using the (n-1)th order Bézier curve.

[0058] Furthermore, when the generated path curve is a Bézier spline curve:

[0059] First, the function p(κ) of the Bézier spline curve is obtained using the (n-1)th order Bézier curve and the scaling factor κ.

[0060] Transform p(κ) into a variation function p along the two-dimensional orthogonal coordinate axes. x (κ) and p y (κ);

[0061] According to p x (κ) and p y (κ) Calculate the radius of curvature R of the Bézier spline curve:

[0062]

[0063] Where, p′ x For p x (κ) is the first derivative with respect to the scaling factor κ; p′ y For p y (κ) is the first derivative with respect to the scaling factor κ; p″ x For p x (κ) is the second derivative with respect to the scaling factor κ; p″ y For p y (κ) is the second derivative with respect to the scaling factor κ;

[0064] By controlling the scaling factor κ, the radius of curvature R is ensured to keep the joint deflection angle within the limit range when the serpentine robotic arm moves along the path.

[0065] Compared with the prior art, the present invention has at least one of the following advantages:

[0066] (1) This invention creatively proposes a time-varying path planning method, which extracts a limited number of feature points to characterize the key information of the dynamic obstacle for obstacles of different positions and shapes, thereby reducing the complexity of the obstacle model and realizing the purpose of real-time analysis of dynamic obstacle information during the path planning process. The feature points on the obstacle surface provide the basis for generating path spline curves.

[0067] (2) This invention accurately provides the optimal direction for bypassing obstacles by constructing a center point O0 and a fuzzy center C. Subsequent path planning only needs to consider the obstacle situation at the location of this direction, which can effectively improve the efficiency of path planning.

[0068] (3) The present invention proposes a method for determining path feature points. The process is accurate and reliable, with strong operability, and can ensure the real-time global obstacle avoidance of the snake-shaped robotic arm.

[0069] (4) Before real-time path planning, the present invention first establishes the transformation relationship between feature points and paths (steps 5, 6, and 7). During real-time path planning, only the real-time data of feature points needs to be input to generate the corresponding spline curve. The spline curve at this time is the motion path of the snake-like robotic arm, which greatly reduces the calculation time.

[0070] (5) By taking advantage of the characteristics of Bézier curves, the present invention can directly plan a smooth path, and then adjust the smooth path in real time according to the nodes on the path and the feature points of dynamic obstacles.

[0071] (6) By taking advantage of the global characteristics of Bézier curves, the present invention can adjust the global path in real time by adjusting the obstacle feature points and curve feature points.

[0072] (7) This invention extracts real-time information of dynamic obstacles and generates corresponding smooth path curves using feature points, enabling real-time obstacle avoidance by the robotic arm. This avoids a large amount of unnecessary computation, improves search efficiency, and can efficiently solve path planning problems under multi-objective or multi-obstacle environments.

[0073] (8) In the path planning process, the starting point direction and ending point direction of the planned path can be adjusted according to the task requirements.

[0074] (9) The method in this invention has multiple adjustable parameters: center point O0 position, number and position of obstacle feature points, scaling center point position, scaling ratio coefficient, etc. Each parameter can be selected or adjusted according to the specific application environment. Therefore, the method in this invention has good environmental adaptability. Attached Figure Description

[0075] Figure 1 This is a flowchart of the time-varying path planning method of the present invention;

[0076] Figure 2 This invention generates a Bézier curve path for rectangular obstacles.

[0077] Figure 3 This invention generates a Bézier curve path for circular obstacles.

[0078] Figure 4 This invention provides a Bézier curve path generated for non-parallelogram obstacles.

[0079] Figure 5 This invention provides a Bézier curve path generated for irregularly shaped obstacles.

[0080] Figure 6 A flowchart for determining the center point that lays the groundwork for generating the path curve in this invention;

[0081] Figure 7 This is a diagram illustrating the Bézier curve generation process when κ = 0.3 according to the present invention.

[0082] Figure 8 This is an obstacle avoidance curve diagram for a multi-obstacle environment according to the present invention;

[0083] Figure 9 This is a diagram illustrating the movement process of the robotic arm in a multi-obstacle environment according to the present invention.

[0084] Figure 10 This is a curve showing the change in the closest distance between the end point of the boom and the obstacle in this invention. Detailed Implementation

[0085] The features and advantages of the present invention will become clearer and more apparent from the following detailed description.

[0086] The term “exemplary” as used herein means “serving as an example, embodiment, or illustration.” Any embodiment illustrated herein as “exemplary” is not necessarily to be construed as superior to or better than other embodiments. Although various aspects of embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless specifically indicated otherwise.

[0087] This invention proposes a time-varying path planning method. For obstacles of different locations and shapes, a finite number of feature points are extracted to characterize the obstacle information, and a spline curve corresponding to the current moment is generated based on the feature points. The path is planned according to the path planning algorithm, and then the path is adjusted in real time according to the nodes on the path and the feature points corresponding to the dynamic obstacles. This allows the snake-like robotic arm to avoid obstacles when moving along the time-varying repeating path.

[0088] This invention employs a time-varying path planning method, the flowchart of which is as follows: Figure 1 As shown:

[0089] Step 1: First, construct the kinematic model of the moving object and input the initial data A: the position of the starting point x. start Direction d start The position of the endpoint x end Direction d end A model of a snake-shaped robotic arm;

[0090] Step 2: Obtain the real-time shape and position information of the obstacles;

[0091] Step 3: Find the center point O0 that serves as the basis for generating the curve;

[0092] Step 4: Extract surface feature points E representing obstacle information from the obstacle surface E. i Obtain the fuzzy center C of the obstacle and determine which side of the obstacle the moving object will travel from.

[0093] Step 5: Based on the established scaling factor k and the magnification center point O0, magnify the surface of the obstacle to obtain set K, and generate the feature points P of the path curve according to a certain rule. i 0 ;

[0094] Step 6: Generate a Bézier curve based on the curve's feature points and output the transient curve at this moment.

[0095] The method of this invention can obtain the path curves corresponding to each time point. In this invention, the method is... Figure 2 , Figure 3 , Figure 4 , Figure 5 The four examples shown illustrate the process and principle of this invention. The x-axis in the figure points from the starting point x... start Point to the endpoint x end The direction is such that the y-axis is perpendicular to the x-axis. This invention is applicable to moving objects such as snake-like robotic arms, redundant robotic arms, continuous robotic arms, mobile vehicles, mobile robots, and remote-controlled aircraft. This invention uses a snake-like robotic arm as an example to specifically describe the process and principle.

[0096] Step 1, input the initial data A, as follows:

[0097] Based on the task requirements, set the starting point x corresponding to the robotic arm's motion path. start With the termination point x end If the starting and ending directions of the path are also constrained, then set the starting point direction d. start With the direction d of the termination point end The relationship between these four parameters is shown in the following formula:

[0098]

[0099] For example Figure 4 d in start With d end Both are -30°. Figure 2 , Figure 3 , Figure 5 d in start With d end All are 0°, so no markings were made.

[0100] Starting point x start End point x end Starting point direction d start , End point direction d endThese initial data are denoted as initial data A.

[0101] In this step, it is also possible to consider building a corresponding DH coordinate system based on the moving object; constructing a transformation matrix and analyzing the transformation relationships between various kinematic parameters. After the path is planned, as the moving object (robotic arm) moves along the path, the changing patterns of various parameters in the moving object (robotic arm) system can be obtained.

[0102] Step 2: Obtain information about the obstacle at this moment and extract key information, as follows:

[0103] (2.1) There are many ways to obtain obstacle information, such as image processing, sparse map construction, etc., which will not be specifically described in this invention;

[0104] (2.2) After obtaining obstacle information, key information needs to be extracted, which is referred to as obstacle feature information in this invention. These obstacle feature information are stored in a matrix, which is denoted as B.

[0105] (2.3)B includes: shape data B(1) of the obstacle occupying one storage space, position data B(2:7) in three-dimensional or two-dimensional space occupying six storage spaces, and size data B(8:); B is an array, and B(2:7) indicates that the data is stored in the array as the 2nd to 7th position.

[0106] Only one data storage is needed to store the shape data of the obstacle; through x, y, z, θ x θ y θ z These six data stores the location data of the three-dimensional obstacles; or via x, y, θ z These three data stores the position data of the two-dimensional obstacle, z, θ x θ y These three data points are always zero; the number of data points required for the size data of obstacles of different shapes is also different. For example, the size data of a spherical obstacle is stored using the radius r, the size data of a cuboid obstacle is stored using the initial data of length a, width b, and height c, and the size data of an irregular polyhedron obstacle is stored using the initial position information of each vertex.

[0107] The shape data B(1) and size data B(8:) of the obstacle do not change over time, but the position data B(2:7) of the dynamic obstacle will change over time, such as due to translation or rotation. Therefore, step 2 needs to be repeated in real time to extract information about the dynamic obstacle in order to obtain initial data for real-time obstacle avoidance.

[0108] In steps 1 and 2, the initial data A that does not need to be updated and the initial data B that needs to be updated in real time were extracted, respectively.

[0109] Step 3, using the two endpoints x in the initial data A start x end Based on the obstacle feature information B, the center point O0 is obtained, which lays the groundwork for the magnification in the subsequent steps. This is specifically achieved in the following way:

[0110] (3.1) Connect the starting point x start With the termination point x end Generate a straight line;

[0111] (3.2) If the straight line does not intersect with any obstacle, i.e., it has no center point O0, then there is no need to consider obstacle avoidance. The moving object can be set to start from the starting point x. start Along this straight line towards the endpoint x end sports;

[0112] (3.3) If a portion of the line segment is inside the obstacle, the two ends of this line segment are connected by the left endpoint x of the obstacle. left and the right endpoint x of the obstacle right This means that the midpoint of a line segment is the center point. Figures 2-5 The flowchart for solving for the center point O0 is shown below. Figure 6 As shown, the specific steps are as follows;

[0113] (3.4) Input the feature information B of the obstacle and the starting point x start End point x end Next, in order to find the left endpoint x of the obstacle left Let the left moving point x left_temp Gradually from x according to the predetermined step size δ start To x end The motion calculation formula is shown below:

[0114] x left_temp =x start +n1·δ

[0115] In the formula, n1 represents the number of steps, n1 = 1, 2, 3, ...

[0116] (3.5) Determine x in each step. left_temp If the object is not within the obstacle space, then analyze whether it has reached or exceeded x. end Point. If yes, then obstacle avoidance is unnecessary; if not reached, then let x... left_temp Continue towards x end The exercise is repeated in cycles (3.4) and (3.5);

[0117] (3.6) If x left_temp Within the obstacle space, the current x left_temp Consider the left endpoint x of the obstacle left ;

[0118] (3.7) After determining the left endpoint x of the obstacle left Next, locate the right endpoint x of the obstacle. right Let the right moving point x right_temp Gradually from x according to the predetermined step size δ end To x start The motion calculation formula is shown below:

[0119] x right_temp =x end -n2·δ

[0120] In the formula, n2 represents the number of steps, n2 = 1, 2, 3, ...

[0121] (3.8) Determine x in each step. right_temp Is it within the obstacle space? If not, let x... right_temp Continue towards x start The exercise was repeated in cycles (3.7) and (3.8);

[0122] (3.9) If x right_temp Within the obstacle space, the current x right_temp Consider the right endpoint x of the obstacle right ;

[0123] (3.10) Determine x left With x right Next, the center point O0 is calculated using the formula shown below:

[0124] O0=(x right +x left ) / 2

[0125] O0 is actually x right With x left The middle point will be used as the center point for scaling up O0 in subsequent steps, and can also be regarded as the center point for path planning of the turning section.

[0126] Step 4: Extract surface feature points E representing obstacle information from the obstacle surface E. i To obtain the fuzzy center C of the obstacle and determine which side of the obstacle the moving object will travel from, the following steps are taken:

[0127] (4.1) Set the number of feature points to be extracted and their positions according to the task. Feature points can be determined based on the starting point x. start With the termination point x endThe location of the inflection point is determined, or the inflection point is used as a feature point, while the type of obstacle and the number of inflection points need to be considered.

[0128] (4.2) Set the inflection points (such as turning points and edges) on the surface of the obstacle as obstacle feature points E. i ,For example Figure 2 E in i These are the feature points of a rectangular obstacle. These points represent the shape and size of the obstacle and represent key information about the obstacle. The fuzzy centroid C can be determined based on these obstacle feature points, which can reduce the amount of calculation.

[0129] Specifically, for obstacles with certain special shapes that have no inflection points, feature points can be set according to established rules, for example, in... Figure 3 In the circular obstacle shown, four straight lines with an included angle of 60° are drawn through point O0. The intersection point E of these straight lines and the circular obstacle is... i These are considered as obstacle feature points. By establishing feature points for circular obstacles using this method, it can be ensured that the generated path curve always lies outside the obstacle.

[0130] Specifically, three E values ​​can be extracted from the triangular obstacle. i Extracting four E's from quadrilateral obstacles i N-sided polygons extract N E i After obtaining these obstacle feature points, their average value is taken as the fuzzy centroid C of the obstacle. The calculation formula is as follows:

[0131]

[0132] In the formula, w represents the number of obstacle feature points, and E i This refers to the location data of the feature points of the obstacle.

[0133] (4.5) By determining the fuzzy centroid C at the starting point x start With the termination point x end The direction of the line determines which side of the obstacle the moving object will traverse, i.e., which side the planned path will bypass the obstacle (whether it's above or below it). If C is in x start With x end If the line connecting the points indicates that most of the space below the line lies beneath the obstacle, then the moving object must travel above the obstacle, meaning the path must be planned from above the obstacle. If C is below x... start With x end The area above the line indicates that most of the space above the obstacle is above the line. Therefore, the moving object should travel below the obstacle, which means planning the path from below the obstacle. Figure 2 , Figure 3 , Figure 4 , Figure 5The fuzzy center of gravity C is below the connecting line, so the path is planned in the upper part of the obstacle area.

[0134] Step 5, extract feature points P of the path curve i 0 The details are as follows:

[0135] (5.1) Let set E be the set of all points on the surface of the obstacle. Enlarge the boundary of the obstacle with point O0 as the center point by a magnification ratio of k. Let set K be the set of all points on the magnified boundary. To avoid the planned path from intersecting with the obstacle, the magnification ratio k must be greater than 1. The larger k is, the farther the planned path is from the obstacle, but too much k will result in a longer path. Considering both obstacle avoidance and minimizing path length, this invention preferably sets the magnification ratio k to 2. Set E and set K satisfy the following relationship:

[0136] K = k·E-O0

[0137] (5.2) The part that requires path planning (e.g.) Figure 2 , Figure 3 , Figure 4 , Figure 5 In x start With x end E (planned path of the upper part of the connecting area) i Zoom in; the zoomed-in points are considered as feature points of the path curve. (For example Figure 2 , Figure 3 , Figure 4 ), or consider the point with the maximum coordinate as a feature point of the curve (e.g. Figure 5 In this step, some obstacles (such as...) are addressed. Figure 5 The points magnified are considered as feature points of the path curve, but the planned path cannot meet the obstacle avoidance conditions. Two solutions can be adopted: (1) increase the magnification ratio k; (2) regard the point with the maximum coordinate as the feature point of the curve, that is, select the extreme endpoint in the magnified set K. Depending on which side of the obstacle the moving object moves from, select the feature point P in the middle of the path curve in the magnified set K. i 0 If the path curve is a Bézier curve, the lines connecting these points form Bézier polygons. This can be specifically represented in an image, for example... Figure 2 The line segments in the equation satisfy the following relationship:

[0138]

[0139] Figure 3 The line segments in the equation satisfy the following relationship:

[0140]

[0141] Figure 4 The line segments in the equation satisfy the following relationship:

[0142]

[0143] Figure 5 The points in the equation satisfy the following relationship:

[0144]

[0145] In the formula, min(K,x) is the minimum value of all points in set K along the x-axis.

[0146] (5.3) Set the starting point x start With the termination point x end Let P1 be the first point among the feature points of the curve. 0 and the last point (A total of n curve feature points);

[0147] (5.4) Passing through x start Point (starting point) with direction d start Start with a straight line in the initial direction, extend this line until it intersects set K, and set this intersection point as the second point in the curve's feature points.

[0148] (5.5) Passing through x end The point (terminal point) is oriented with direction d. end Extend the straight line (in the termination direction) until it intersects set K, and set the intersection point as the second to last point in the curve's feature points.

[0149] (5.6) Several points in the middle of the characteristic points of the curve are obtained through (5.1) and (5.2): The first point P1 in the curve feature points was set by (5.3). 0 and the last point The second point among the characteristic points of the curve is obtained through (5.4). The second-to-last point among the characteristic points of the curve is obtained through (5.5). In summary, based on the obstacle feature points E obtained in step 4 i Based on the direction of path planning, all curve feature points P were obtained. i 0 .

[0150] Step 6, based on the curve feature point P i 0 Generate a Bézier curve as follows:

[0151] The curve representing the path can be an arc, a sine or cosine curve, an Nth-order polynomial curve, a Bézier spline curve, a B-spline curve, etc. Bézier spline curves and B-spline curves are formed by fitting these feature points, while the other curves are formed by interpolating these feature points.

[0152] This invention uses Bézier curves as an example to illustrate how to plan time-varying paths using curve feature points.

[0153] Based on the obstacles, n feature points can be obtained: Using these feature points and the scaling factor κ, an (n-1)th order Bézier curve can be generated. From P1 0 arrive In this invention, a polygon formed by the broken lines created by each feature point is referred to as a feature polygon. Figure 7 Taking κ = 0.3 as an example, the points after the first iteration satisfy the following relationship:

[0154]

[0155] The points after the second iteration satisfy the following relationship:

[0156]

[0157] After the third iteration, the points on the Bézier curve can be obtained, which satisfy the following relationship:

[0158]

[0159] The points and line segments in the diagram satisfy the following relationship:

[0160]

[0161]

[0162] In the formula P i j Let p(κ) be the i-th point obtained after the j-th iteration, and p(κ) be a function representing the Bézier curve, where κ∈[0,1]. When κ changes from 0 to 1, a curve like p(κ) is generated. Figure 7 The figure shows a cubic Bézier curve defined by n=4 vertices.

[0163] By analogy, the nth-degree Bézier curve p(κ) defined by n+1 vertices satisfies the following equation:

[0164]

[0165] In the formula In probability theory, denoted as the number of combinations.

[0166] The p(κ) corresponding to the Bézier curve can be rearranged into the matrix form shown below:

[0167] p(κ)=PK1(1-κ)K2(κ)H T

[0168] The matrices P, K1, K2, and H in the formula are as follows:

[0169]

[0170] K1(1-κ)=[(1-κ) n ,(1-κ) n-1 ,…,(1-κ) 0 ]·I n

[0171] K2(κ)=[κ 0 ,κ 1 ,…,κ n ]·I n

[0172] H T =[λ1,λ2,…,λ n ]

[0173] In the formula, P is the geometric matrix of the feature points; K1 and K2 are the diagonal matrices related to the parameter κ; and H is the weight matrix. These are the weighting coefficients.

[0174] Differentiating the variables K1, K2, and p(κ) with respect to κ, we get:

[0175]

[0176]

[0177]

[0178]

[0179] By simply substituting the specific value of κ into the above derivative formula, the matrix related to κ can be solved in advance. Adjusting the weight matrix according to the desired result yields the final improved curve p(κ) and its first and second derivatives, which is the final Bézier curve at this moment.

[0180] In two-dimensional space, the curve p(κ) changes along the x-axis and y-axis, and the corresponding change function can be written as p x (κ) and p y (κ). The formula for calculating the radius of curvature R corresponding to this curve is as follows:

[0181]

[0182] In the formula, p′ x For p x (κ) is the first derivative with respect to the parameter κ; p′ y For p y (κ) is the first derivative with respect to the parameter κ; p″ x For p x (κ) is the second derivative with respect to the parameter κ; p″ y For p y (κ) is the second derivative with respect to the parameter κ.

[0183] By controlling the parameters in the formula, the radius of curvature R of the Bézier curve can always be kept within the specified range, thus ensuring that the joint deflection angle is always within the limit range when a moving object, similar to a snake-like robotic arm, moves along the path.

[0184] After step 6 is executed, the Bézier curve corresponding to this moment is generated. In step 7, the Bézier curve obtained at this moment is used as the transient motion path and output as the motion path of the moving object. Then, steps 2, 3, 4, 5, 6, and 7 are repeated at the next moment.

[0185] The above process describes the generation of obstacle feature points, path feature points, and ultimately, path curves for a single dynamic obstacle. Using the method in this invention, the process of generating a Bézier curve is as follows: feature points are generated for each obstacle; the main obstacle feature points are selected; then curve feature points are generated; and based on these feature points, a smoothly transitioning Bézier curve is generated. When a moving object moves along this path, dynamic obstacle avoidance can be achieved. The method and principle for generating Bézier curves for multiple obstacles are similar to those for generating Bézier curves for a single obstacle, and will not be described in detail here.

[0186] In a multi-obstacle environment, the obstacle avoidance curve generated at a certain moment is as follows: Figure 8 As shown in the figure, there are "L-shaped obstacles", "rectangular obstacles", "triangular obstacles", "circular obstacles" and "pentagram obstacles". When these five obstacles move along the movement path, the method in this invention generates Bézier curves in real time, thereby ensuring that the moving object can avoid obstacles during the movement.

[0187] The above describes the process of a time-varying path planning method based on Bézier curves corresponding to this invention. The effectiveness and practicality of this invention will be demonstrated below through theoretical derivation and simulation.

[0188] First, the effectiveness and practicality of this invention are proven through theoretical derivation. The path planned using this invention ensures that the moving object will never collide with obstacles, and the distance between the moving object and the obstacle can be adjusted by changing the magnification factor (magnification ratio k). The proof process is described below using a circular obstacle as an example.

[0189] If O0 is taken as the origin, then Figure 3 The coordinates of the four points E1, E2, E3, and E4 are as follows:

[0190]

[0191] In the formula, r represents the size information of the circular obstacle: the radius of the circle; y0 is the distance between point O0 (center point) and point C (fuzzy centroid).

[0192] The method described in this invention yields six feature points, from which the expression for a fifth-order Bézier curve can be derived:

[0193]

[0194] Substitute From these 6 points, we can analyze that when κ = 0.5, the distance between the Bézier curve and the obstacle is the shortest. Let κ = 0.5, and rearrange to obtain:

[0195]

[0196] Ensure the curve lies outside the obstacle, i.e.:

[0197]

[0198] After sorting, we can obtain:

[0199]

[0200] Substituting ξ and rearranging, we get:

[0201]

[0202] Let y0 = k·r, where k ∈ (0,1). Substituting and simplifying, we get:

[0203]

[0204] Further analysis reveals:

[0205]

[0206] Let f(k) be the left-hand side of the inequality in the above equation, that is:

[0207]

[0208] Taking the derivative of f(k), and analyzing the monotonicity of the function on the interval (0,1), we can obtain the following results on the interval... The function is increasing between and , and in the interval . The function is decreasing, and the two minimum points f(k=0)=1.32 and f(k=1)=0 both satisfy the condition that they are greater than or equal to 0. Through the theoretical derivation here, we can conclude that the Bézier curve never touches any obstacles.

[0209] Next, simulations will be used to demonstrate the effectiveness and practicality of this invention. Using simulation software, a motion path is generated by inputting all feature points of five obstacles: an "L-shaped obstacle," a "rectangular obstacle," a "triangular obstacle," a "circular obstacle," and a "pentagonal obstacle." The method described in this invention can generate obstacle feature points, curve feature points, and Bézier curves in real time. As the serpentine robotic arm moves along the real-time path, it can ensure that it never touches any obstacles. The path generated at a certain moment is as follows: Figure 8 As shown in the figure, the asterisks (*) represent feature points of each obstacle, the curves represent the movement paths of each obstacle, and the shapes surrounding the obstacles represent their magnified boundaries. The motion process of the robotic arm in a multi-obstacle environment is illustrated in the following diagram. Figure 9 As shown in the figure, the motion state of the robotic arm at different times is illustrated. During the motion, the example object in this invention is a robotic arm with a total of 8 links. The shortest distance d between the end point of the i-th link (also referred to as the center point of the i-th joint) and all obstacles is... i The curve showing the change in distance between the end point of the boom and the obstacle is as follows: Figure 10 As shown, Figure 10 In the diagram, the moving object (robotic arm) has a total of 8 arms. The shortest distance between the end point of the i-th arm and all obstacles is d. i In the figure, d8 is the closest distance between the key point (the end point of each arm, or the joint center point) and the obstacle. It can be seen from the figure that the distance is always greater than zero.

[0210] If there is one or more obstacles in the movement space, this invention can ensure that the obstacles and the robotic arm (or the real-time movement path) always maintain a certain distance in real time.

[0211] This invention is particularly applicable to path planning for moving objects such as snake-shaped robotic arms, redundant robotic arms, continuous robotic arms, mobile vehicles, mobile robots, and remote-controlled aircraft in dynamically changing single-obstacle environments and dynamically changing multi-obstacle environments.

[0212] The present invention has been described in detail above with reference to specific embodiments and exemplary examples; however, these descriptions should not be construed as limiting the present invention. Those skilled in the art will understand that various equivalent substitutions, modifications, or improvements can be made to the technical solutions and embodiments of the present invention without departing from the spirit and scope of the invention, and all such modifications and improvements fall within the scope of the present invention. The scope of protection of the present invention is defined by the appended claims.

[0213] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A time-varying path planning method, characterized in that, include: S1 determines the initial path data of the robotic arm based on the task requirements; The initial path data includes the starting point of the path. x start and termination point x end ; S2 obtains the feature information of obstacles at the current moment; obstacles The feature information includes obstacle shape data, obstacle location data, or obstacle size data; The obstacles are dynamic obstacles, and their position data changes over time; S3 determines whether obstacle avoidance is needed based on the initial path data of the robotic arm and the characteristic information of the obstacle; When obstacle avoidance is not required, generate a path and end path planning; When obstacle avoidance is required, the center point of the portion of the path that does not require obstacle avoidance within the obstacle space is obtained based on the initial path data of the robotic arm and the characteristic information of the obstacle. O 0, Proceed to step S4; S4 acquires the feature points of the obstacle and determines the fuzzy center of the obstacle based on the feature points. C Based on the fuzzy center of the obstacle C The initial path data of the robotic arm determines the direction in which the path bypasses obstacles; S5 based on the center point O 0 and the direction in which the path bypasses obstacles determine the feature points of the path; S6 generates the current path based on the feature points of the path; In the next moment, S7 returns to step S2; In step S6, the path curve (Bézier spline curve) at the current moment is generated; Generate based on the feature points of the path n -1st order Bézier curve, using n The -1st order Bézier curve determines the Bézier spline curve; the scaling factor is controlled so that the radius of curvature satisfies the condition that the joint deflection angle is always within the limit range when the snake-like robotic arm moves along the path. Based on the fuzzy center of the obstacle C Methods for determining the direction of a path around an obstacle include: Connect the starting point x start and the end point x end Generate a straight line; If the fuzzy center of the obstacle C If the path bypasses the obstacle on one side of the straight line, the feature point of the obstacle located on that side is recorded as the valid feature point of the obstacle. If the fuzzy center of the obstacle C If the path falls on the straight line, the path will bypass the obstacle from either side of the line.

2. The time-varying path planning method according to claim 1, characterized in that, In step S1, the initial data also includes the starting point direction. d start Or the direction of the termination point d end .

3. The time-varying path planning method according to claim 1, characterized in that, In step S3, the method for determining whether obstacle avoidance is needed based on the initial path data of the robotic arm and the feature information of the obstacle includes: Connect the starting point x start and the end point x end Generate a straight line, and determine whether the line intersects with the obstacle based on the obstacle's feature information. If they do not intersect, it is determined that no obstacle avoidance is required; if they intersect, it is determined that obstacle avoidance is required. When obstacle avoidance is not required, use the straight line as the generated path and end the path planning. When obstacle avoidance is required, the midpoint of the line segment where the straight line intersects the obstacle is taken as the center point. O 0.

4. The time-varying path planning method according to claim 3, characterized in that, The method in step S3 includes: S3.1 Set the left moving point x left_temp The initial position is x start ; , S3.2 Based on the feature information of the obstacle, determine x left_temp Whether it falls into the obstacle space; if it falls into the obstacle space, then... x left_temp The first endpoint of the line segment where the straight line intersects the obstacle. x left Proceed to step S3.

4. If the obstacle is not within the obstacle space, proceed to step S3.

3. S3.3 Order x left_temp Towards x end stride length δ ; judge x left_temp Whether arrived or exceeded x end If arriving or exceeding x end The output will be as follows x start and x end For a straight-line path between two endpoints, terminate path planning if the destination is not reached or the path exceeds the limit. x end Then return to step S3.2; S3.4 Setting the right moving point x right_temp The initial position is x end ; S3.5 Based on the feature information of the obstacle, determine x right_temp Whether it falls into the obstacle space; if it falls into the obstacle space, then... x right_temp The second endpoint of the line segment where the straight line intersects the obstacle. x right Proceed to step S3.

7. If the obstacle is not within the obstacle space, proceed to step S3.

6. S3.6 Order x right_temp Towards x start stride length δ And return to step S3.5; S3.7 will be x left and x right The midpoint of the line segment with two endpoints is taken as the center point. O 0.

5. The time-varying path planning method according to claim 1, characterized in that, In step S4, the feature points of the obstacle include the inflection points of the obstacle; Fuzzy center of obstacle C The coordinates are the average of the coordinates of the feature points of each obstacle.

6. The time-varying path planning method according to claim 5, characterized in that, In step S4, when the two-dimensional obstacle is a shape without corners, the feature points of the obstacle are determined according to the following method: Connect the starting point x start and the end point x end Generate the first straight line; Passing through the center point O 0th work, 2nd~ N A straight line, N In a straight line, the angle θ between two adjacent straight lines is θ = 180° / ( N- 1); N ≥3 ； Will N The intersection of a straight line with the boundary of an obstacle is taken as the feature point of the obstacle; When a two-dimensional obstacle is a polygon, the feature points of the obstacle include the vertices of the polygon.

7. The time-varying path planning method according to claim 1, characterized in that, Let the feature points of the path be... , ; In step S5, based on the center point O Methods for determining the feature points of a path based on the direction of the path around obstacles include: S5.1 will x start and x end As respectively and ; S5.2 with center point O Let 0 be the center point, and magnify the boundary of the obstacle. Let the magnified boundary and the set of points inside it be denoted as set. K ; S5.3 passed x start Draw a straight line from the point along a first specific direction, and extend this line until it intersects with the set. K If they intersect, take the intersection point as... ;Pass x end Draw a straight line along the second specific direction from the point, and extend the line until it intersects with the set. K If they intersect, take the intersection point as... ; When a starting point direction is specified d start Or the direction of the termination point d end When constrained, the first specific direction and the second specific direction are respectively d start and d end ; When the starting point direction is not set d start Or the direction of the termination point d end When constrained, both the first and second specific directions are through... x start Dot and x end The direction of the line connecting the points; S5.4 will set K In the diagram, the points corresponding to the valid feature points of the obstacle are used as... ~ ; Alternatively, establish a center point O 0 is the origin. x start Dot and x end The direction of the line connecting the points is x axis, + y Axis perpendicular to x Let the coordinate system of the axes point in the direction in which the path bypasses the obstacle. , , ; in, For set K All points in x Minimum value in the axial direction, For set K All points in y Maximum value in the axial direction, For set K All points in x Maximum value in the axial direction.

8. The time-varying path planning method according to claim 1, characterized in that, When the generated path curve is a Bézier spline curve: First use n -1st order Bézier curve and scaling factor κ Functions that yield Bézier spline curves p ( κ ); Will p ( κ Transform into a function of variation along two-dimensional orthogonal coordinate axes. p x ( κ )and p y ( κ ); according to p x ( κ )and p y ( κ Calculate the radius of curvature of a Bézier spline curve. : ； in, for p x ( κ ) and proportional factors κ The first derivative; for p y ( κ ) and proportional factors κ The first derivative; for p x ( κ ) and proportional factors κ The second derivative; for p y ( κ ) and proportional factors κ The second derivative; Control Scale Factor κ , making the radius of curvature R The joint deflection angle must always be within the limit range when the serpentine robotic arm moves along the path.

Images