U-turn path optimization method based on equidistant spiral motion model

Abstract

The invention relates to the technical field of automatic control and path planning, in particular to a U-turn path optimization method based on an equidistant spiral motion model, which comprises the following steps of: 1, establishing the equidistant spiral motion model; 2, arc design of a U-turn path; step 3, collision detection and path optimization; and step 4, optimizing a U-turn path. By introducing the equidistant spiral line model and the tangent relation between the equidistant spiral line model and the arc, the U-turn path can be optimized on the premise of no collision, the required space is reduced, and the path smoothness is improved. The method can be applied to robots, automatic driving and various automatic trajectory planning systems, and is particularly suitable for path planning and turning-around operation of a multi-body system.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic control and path planning, and in particular to a U-turn path optimization method based on an equidistant spiral motion model. Background Art

[0002] In automation systems, path planning and turning of multi-segment moving bodies are common technical problems. Especially in limited spaces, how to design the optimal turning path without collision is directly related to the system's motion efficiency and safety. In the prior art, geometric methods or heuristic search algorithms are usually used to achieve path planning, but in multi-body motion, there are still many deficiencies in how to ensure the smoothness of the turning path, reduce the required space, and improve safety.

[0003] The equidistant spiral model has gradually attracted attention due to its unique geometric properties. It can not only ensure the smoothness of movement, but also achieve global path optimization through parameter adjustment. However, in the problem of U-turn path optimization, there are few studies on the combination of equidistant spirals and traditional arc paths. The present invention is aimed at this problem. By introducing the tangent relationship between the equidistant spiral and the arc path, the precise control of the U-turn path is achieved, and the U-turn path is minimized under the premise of ensuring that the moving body does not collide.

[0004] Path planning methods in the prior art generally include geometry-based path generation methods, search-based algorithms, and the like. Whether it is a geometric method, a search algorithm, or a bio-inspired algorithm, it is easy to fall into a local optimal solution in path planning, resulting in the inability to find a global optimal path. When dealing with dynamic obstacles, the computational efficiency and response speed of existing algorithms are insufficient to meet the needs of real-time path updates. However, in the face of complex curved motion and dynamic obstacle avoidance, traditional methods are difficult to adapt to actual needs. In order to solve these problems, the present invention proposes a U-turn path optimization method based on an equidistant spiral motion model, which realizes real-time collision detection and path optimization while ensuring the global optimal path. Summary of the invention

[0005] The purpose of the present invention is to solve the shortcomings existing in the prior art, and to propose a U-turn path optimization method based on an equidistant spiral motion model. This method introduces an equidistant spiral model and a method of tangent to two arcs to ensure that the moving body can successfully complete the U-turn in a limited space. In the optimization process, the U-turn path is minimized as the goal, combined with a collision detection model, and the arc radius is dynamically adjusted to finally obtain the shortest U-turn path.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] A U-turn path optimization method based on an equidistant spiral motion model, the specific steps are as follows:

[0008] Step 1: Establish an isometric spiral motion model;

[0009] Step 2: Arc design of the U-turn path;

[0010] Step 3: Collision detection and path optimization;

[0011] Step 4: U-turn path optimization.

[0012] Preferably, in step 1, an equidistant spiral is used as the motion trajectory of the moving body, and its polar coordinate equation is:

[0013] r=r0+k·θ

[0014] Among them, r0 is the initial radius, k is the pitch, and θ is the movement angle of the moving body; the equation shows that as the angle increases, the moving body moves along an equidistant spiral trajectory, ensuring the smoothness of the motion trajectory and optimal use of space.

[0015] Preferably, in step 2, an S-shaped path with two tangent arcs is used, the radius ratio of the front and rear arcs is 2:1, and they are tangent to the equidistant spiral line; the tangent point of the arc and the spiral line is determined by geometric calculation, and it is ensured that the path of the entire turning process is smooth and without mutations.

[0016] Preferably, in step 3, geometric analysis is used to determine whether the moving body collides; an improved particle swarm optimization algorithm is used, with minimum pitch as the objective function, a single-objective optimization model is established, and path parameters are dynamically adjusted to ensure that path planning is completed without collision.

[0017] Preferably, in step 4, the path parameters are dynamically adjusted by optimizing the target and the collision detection model, and the optimal U-turn path is output.

[0018] Compared with the prior art, the present invention has the following beneficial effects:

[0019] 1. The present invention introduces an equidistant spiral model and its tangent relationship with the arc, so as to optimize the turning path, reduce the required space and improve the path smoothness under the premise of no collision.

[0020] 2. The present invention adopts an equidistant spiral motion model for path planning and combines it with an improved particle swarm algorithm to effectively deal with trajectory planning and real-time obstacle avoidance problems in complex environments.

[0021] 3. The present invention can be applied to robots, autonomous driving and various automated trajectory planning systems, and is particularly suitable for path planning and turning operations of multi-body systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 is a schematic diagram of a U-turn path in an embodiment of the present invention;

[0023] Figure 2 This is a diagram of the equidistant spiral motion trajectory of the robot regarding turning in an embodiment of the present invention. DETAILED DESCRIPTION

[0024] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings, so that those skilled in the art can better understand the advantages and features of the present invention, thereby making a clearer definition of the protection scope of the present invention. The embodiments described in the present invention are only a part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present invention.

[0025] Example: Factory Automation Logistics System

[0026] Robots need to carry items in limited spaces such as narrow aisles and warehouses. In large warehouses, robots often need to turn around or change driving directions in these small spaces. When the number of robots is too large, the possibility of collisions will increase. At this time, if the robot's driving path is set and the turning area is reasonably planned, not only can the space occupied by the robot when turning can be reduced, but also the stability of movement can be maintained, and the falling of items or damage to the robot caused by sharp turns can be avoided. With respect to the selection of the turning path, the present invention proposes a method of making two circular arcs tangent to equidistant spirals. The purpose is to complete the U-turn in the specified U-turn space, while making the U-turn path as short as possible and ensuring that no collision occurs during the U-turn process. The specific solution process is as follows:

[0027] Constraint 1: The U-turn path consists of two tangent arcs.

[0028] Constraint 2: The radius of the front arc is twice the radius of the back arc.

[0029] Objective function: The goal is to minimize the arc length of the U-turn path, that is, to find the shortest U-turn path by optimizing the arc radius of the U-turn area. The specific objective function is as follows:

[0030]

[0031] Where L is the total turn path length, r i are the radii of the two arcs, θ i is the corresponding central angle.

[0032] Under the above two constraints, the objective function optimization scheme is optimized by traversing the radius of the U-turn area through the variable step size enumeration method, and gradually adjusting the radius value until the minimum path solution that meets the constraints is found.

[0033] In view of the above problems, the calculation steps of the present invention are as follows:

[0034] Step 1: The main goal of this step is collision detection

[0035] Given the coordinates of the robot's vertices relative to the origin of the ground coordinate system, use the vector product method to determine whether a collision has occurred.

[0036] The vector product determines the position of a point relative to a line segment by calculating the product of two vectors. A positive vector product indicates that the point is on the left, a negative vector product indicates that the point is on the right, and a zero vector product indicates that the point is on the line. This method is widely used in geometric calculations, such as determining whether a point is inside a polygon or for collision detection of polygons. For a point in a plane coordinate system, let point A = (x A ,y A ), B=(x B ,y B ), P is a point on another rectangle, let P = (x, y), then In the plane rectangular coordinate system oxy, the basis is {i, j}, which is expanded to a set of bases {i, j, k} of the space rectangular coordinate system oxyz. Then in the space rectangular coordinate system Then the vector product is:

[0037]

[0038] If P is on the left side of AB, the direction of the vector product is the same as the positive direction of the z-axis; if P is on the right side of AB,

[0039] Then the direction of the vector product is the same as the negative direction of the z-axis; if P is on AB, then and If they are collinear, the vector product is 0. Therefore, the present invention only needs to determine whether the robot vertex is behind its adjacent robot to determine whether to send a collision.

[0040] Step 2: Determine the robot's turning area

[0041] With the robot's position as the center of the circle, let the distance between two adjacent robots be x, and the distance between the robot and the nearest object in the warehouse be y. If The radius of the turning area is y; otherwise, The radius of the turning area is

[0042] Step 3: Create an equidistant spiral model to determine the robot's turning path

[0043] According to the definition of equidistant spiral, its parametric equation is:

[0044]

[0045] Where r0 is the initial radius of 0, k is the pitch, and θ is the angle. a is a constant that represents the increment of the radius of each turn of the spiral. The equation of the equidistant spiral symmetrical about the origin is:

[0046] r(θ)=a·(θ+π)

[0047] The pitch can be determined based on the tangency conditions of each arc and the geometric relationship. It is known that the "S"-shaped path of the U-turn is tangent to the spiral motion trajectory at the initial U-turn point C, and the radius of the first arc is twice that of the second arc. Therefore, the equidistant spiral motion trajectory of the robot about the U-turn can be drawn.

[0048] In Figure 2, O1 and O2 represent the centers of the two arcs in the figure, CD is a straight line passing through the origin O1 of the first arc and perpendicular to the tangent passing through point C, r1′ and r2′ represent the radii of the two arcs and satisfy r1′=2r2′. θ1 is the angle between the first arc and the straight line CD, and θ2 is the angle between the straight line connecting r1′ and r2′ and the straight line CD.

[0049] According to the equidistant spiral equation and C(x c ,y c ) coordinates, and the tangent equation of the "S"-shaped path tangent to it is obtained as:

[0050]

[0051] Since the two arcs of the U-turn path are tangent to the equidistant spiral, the two tangent equations can be deduced to be parallel based on the properties of the equidistant spiral, and the equation of the other tangent can be obtained. Since CD is perpendicular to the two tangents, then:

[0052] CD=2Rcosθ1

[0053] Among them, R is the radius of the turning space, and because CD passes through point C, the equation of the CD line is:

[0054]

[0055] Given the coordinates of point C and point B, find the straight-line distance d1 between points C and B:

[0056]

[0057] Similarly, the distance formula can be used to calculate the lengths of CD, BD, and BC. Using the cosine theorem, the angle of θ1 is:

[0058] 2BC·CDcosθ1=BC 2 +CD 2 -BD 2

[0059] According to the relationship between the internal and external angles of a triangle, the angle between the straight line CD and the straight line connecting r1′ and r2′ is twice the internal angle θ1, that is: θ2=2θ1. Through the above relationship and conditions, we can get (r1′+r2′)sinθ2=2Rsinθ1, and we can get:

[0060]

[0061] Wherein, R is the radius of the turning area, and θ1 is the angle between the first arc and the straight line CD.

[0062] Step 4: Calculate the shortest U-turn path

[0063] A single-objective optimization model is established with the minimum length of the U-turn path as the objective function and the tangent relationship of each arc as the constraint condition. The radius of the arc is adjusted using the variable step enumeration method to shorten the U-turn path, and the shortest U-turn path is finally determined. According to the different results R detected, the radius of the U-turn curve is adjusted in time to successfully complete the U-turn and reduce the possibility of collision.

[0064] In summary, the U-turn path optimization method based on the equidistant spiral motion model proposed in the present invention solves the path planning and collision detection problems in complex environments; through the improved particle swarm optimization algorithm, efficient and accurate path optimization is achieved. This method can be widely used in robot navigation, automatic driving, drone control and other fields, and has good practical value and promotion prospects.

[0065] The description and practice disclosed in the present invention are easy to think and understand for ordinary technicians in the technical field, and several improvements and modifications can be made without departing from the principles of the present invention. Therefore, modifications or improvements made without departing from the spirit of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A U-turn path optimization method based on an equidistant spiral motion model, characterized in that: The following steps are involved: Step 1: Establish an isometric spiral motion model; Step 2: Arc design of the U-turn path; Step 3: Collision detection and path optimization; Step 4: U-turn path optimization.

2. The U-turn path optimization method based on the equidistant spiral motion model according to claim 1 is characterized in that: In step 1, an equidistant spiral is used as the motion trajectory of the moving object, and its polar coordinate equation is: r=r0+k·θ Among them, r0 is the initial radius, k is the pitch, and θ is the movement angle of the moving body; the equation shows that as the angle increases, the moving body moves along an equidistant spiral trajectory, ensuring the smoothness of the motion trajectory and optimal use of space.

3. The U-turn path optimization method based on the equidistant spiral motion model according to claim 1 is characterized in that: In step 2, an S-shaped path with two tangent arcs is used. The radius ratio of the front and rear arcs is 2:1, and they are tangent to the equidistant spiral line. The tangent point of the arc and the spiral line is determined by geometric calculation, and it is ensured that the path of the entire U-turn process is smooth and without mutations.

4. The U-turn path optimization method based on the equidistant spiral motion model according to claim 1 is characterized in that: In step 3, geometric analysis is used to determine whether the moving body collides. An improved particle swarm optimization algorithm is used to establish a single-objective optimization model with the minimum pitch as the objective function, and the path parameters are dynamically adjusted to ensure that the path planning is completed without collision.

5. The U-turn path optimization method based on the equidistant spiral motion model according to claim 1 is characterized in that: In step 4, the path parameters are dynamically adjusted by optimizing the target and collision detection model, and the optimal U-turn path is output.

Images