Snake robot path planning method based on fuzzy collision constraint and spiral trajectory

Abstract

This invention discloses a path planning method for a snake robot based on fuzzy collision constraints and helical trajectory, comprising the following steps: S1, establishing an overall path model using the Archimedes' spiral equation; S2, obtaining the complete motion trajectory of the multi-node system through solving the state of the first node and recursion between nodes; S3, using a fuzzy collision risk function for flexible judgment to avoid path interference between the first segment and subsequent segments; S4, using the pitch as the optimization variable and employing a simulated annealing algorithm to find the optimal path parameters to ensure safe entry into the target area. The trajectory generated by this invention is smooth and continuous, can actively predict collisions, and can quantitatively optimize path parameters, making it particularly suitable for scenarios with high requirements for motion smoothness and safety, such as narrow pipe inspection, complex terrain exploration, space-constrained situations, or the need to maneuver into specific turning spaces.

Description

Technical Field

[0001] This invention belongs to the field of snake robot motion planning and control technology, and more specifically, relates to a snake robot path planning method based on fuzzy collision constraints and spiral trajectory. Background Technology

[0002] Snake-like robots, due to their high segmental flexibility and maneuverability, are widely used in disaster search and rescue, underground pipeline inspection, and complex terrain exploration. However, the motion of such systems is not the independent movement of each node, but rather a highly coupled cooperative motion. Snake-like robots consist of multiple serially connected segments, and their motion coordination places high demands on path continuity and spatial adaptability.

[0003] In confined spaces, traditional path planning methods (such as straight-line splicing and spline interpolation) struggle to balance trajectory continuity, collision avoidance, and spatial compactness when turning around or maneuvering into narrow areas. This can easily lead to segment overlap or execution failure, impacting task safety and efficiency. Specifically, when implementing complex curved movements such as spiraling or turning around in narrow spaces, the paths planned by traditional methods are often not smooth or natural enough, lacking overall coherence and prone to motion jitter or low energy efficiency. Secondly, it is difficult to accurately predict whether collisions will occur between nodes during continuous movement, resulting in insufficient safety assurance. Furthermore, when the movement space is limited, there is a lack of systematic parameter optimization methods to find a safe and feasible movement path.

[0004] Therefore, there is an urgent need in this field for a path generation method that can adapt to configuration constraints, has high continuity and optimization capabilities, and is specifically designed for the safety planning problem of multi-segment serial systems such as snake robots in confined spaces. Summary of the Invention

[0005] To address the aforementioned issues, this invention proposes a snake robot path planning method based on fuzzy collision constraints and spiral trajectories. The aim is to utilize the mathematical properties of the Archimedean spiral to generate an overall and continuous optimal motion path for a multi-node connected body, and to predict and avoid collision risks between nodes during the planning stage. This method is particularly suitable for motion scenarios under complex constraints.

[0006] To address at least one of the aforementioned technical problems, according to one aspect of the present invention, a snake robot path planning method based on fuzzy collision constraints and helical trajectories is provided, comprising the following steps:

[0007] Assume the system consists of It consists of nodes, with the first node being the head node and the rest being subsequent nodes. Each node has a length of . Width is The distance between nodes is The distance from the front end of the previous node is The distance from the last node to the tail is .

[0008] Step S1, Motion Trajectory Modeling:

[0009] The overall motion path model of the multi-node series body is established based on the Archimedean spiral. The polar coordinate equation of the Archimedean spiral is shown in formula (1):

[0010]

[0011] Wherein, the polar diameter of the helix is The initial polar diameter of the helix is The coefficient of variation of the polar radius per unit rotation angle is The angles corresponding to each node on the spiral are The initial angle of rotation is The model defines the continuous motion trajectory of the first node starting from its initial position.

[0012] Step S2, Node State Calculation:

[0013] Step S2.1, First Node Motion Calculation: Set the first node to move along the Archimedean spiral at a constant linear velocity. Motion. According to the differential formula for arc length in polar coordinates:

[0014]

[0015] Establish the first node polar angle using the aforementioned helical equation. Over time The changing differential equation:

[0016]

[0017] Numerical integration methods, such as the fourth-order Runge-Kutta method, are used to solve the differential equation, and the polar angles of the first node at different times are obtained. Then, its position coordinates are obtained through the spiral equation.

[0018] Step S2.2, Subsequent node position deduction: Based on the fixed physical distance between adjacent nodes (e.g., segment length), establish geometric constraint equations. Given the first... The position of each node and polar angle By solving the following system of equations, the first equation can be obtained recursively. Polar angle of each node and location :

[0019]

[0020] in, The distance between adjacent nodes. This is the Archimedes' spiral equation. By traversing all nodes and time points, the complete pose sequence of the entire chain during motion is obtained, ensuring that the overall chain motion conforms to the series constraint relationship.

[0021] Step S3, Collision Risk Assessment:

[0022] A fuzzy logic mechanism is introduced for collision detection, which calculates the Euclidean distance between the apex of the first node (whose position is determined by the node width, the offset of the previous node, and the orientation) and the midpoint of the center line segment of any subsequent node.

[0023]

[0024] Constructing a fuzzy collision risk function:

[0025]

[0026] Transform collision detection into a soft constraint form. If If the system determines that there is a potential collision risk, it will record the current state as the upper limit of trajectory feasibility.

[0027] Step S4, Path Parameter Optimization:

[0028] When the space for motion is limited, the parameters of the Archimedean spiral are optimized. The pitch of the Archimedean spiral is used as a reference. To optimize the variables, an evaluation function is introduced:

[0029]

[0030] in To minimize the risk of fuzzy collisions, The Euclidean distance between the trajectory endpoint and the target region boundary is minimized using a simulated annealing algorithm. The algorithm comprises four stages: perturbation generation, probability acceptance, temperature reduction, and termination determination. The final output is the optimal pitch. This achieves a unified approach to spatial integration, path continuity, node coordination, and collision avoidance, thereby generating an optimal path adapted to a specific space.

[0031] According to another aspect of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps of the snake robot path planning method based on fuzzy collision constraints and helical trajectories of the present invention.

[0032] According to another aspect of the present invention, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the snake robot path planning method based on fuzzy collision constraints and helical trajectories of the present invention.

[0033] Compared with existing technologies, the beneficial effects of the above-described method of the present invention are as follows:

[0034] The path of this invention is continuous and differentiable, naturally smooth, and has a clear geometric expression, making it particularly suitable for use in segmental series systems.

[0035] The fuzzy function introduced in this invention can perform continuous and differentiable risk modeling of the relative distance between different segments, avoiding unexpected jumps in the judgment result near the boundary, thereby improving the stability of collision detection and the safety of path judgment.

[0036] The optimized algorithm incorporates parameter control, which can automatically search for the optimal path parameters (such as minimum pitch) for specific spatial constraints (such as the size of the turning area), realizing the quantitative evaluation and optimization of the feasibility of movement in a limited space. It has good adaptability and portability, which is difficult to achieve with traditional methods. It is suitable for tasks such as path planning, turning and posture adjustment and compact arrangement of snake robots in a limited space. Attached Figure Description

[0037] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings of the embodiments will be briefly described below. Obviously, the drawings described below only relate to some embodiments of the present invention and are not intended to limit the present invention.

[0038] Figure 1 This is a general flowchart of a preferred embodiment of the present invention;

[0039] Figure 2 This is a schematic diagram showing the position distribution of the first node of a snake-like robot at different times on a spiral trajectory, according to a preferred embodiment of the present invention.

[0040] Figure 3 This is a schematic diagram illustrating the recursive positioning of multiple nodes of a snake robot at different times according to a preferred embodiment of the present invention.

[0041] Figure 4 This is a schematic diagram of a snake robot colliding according to a preferred embodiment of the present invention;

[0042] Figure 5 This is a schematic diagram of the motion trajectory of a snake robot with a fixed pitch when entering the terminal disk according to a preferred embodiment of the present invention;

[0043] Figure 6This is a schematic diagram of the nodes of the optimal collision-free path for a snake robot to enter a confined space, according to a preferred embodiment of the present invention. Detailed Implementation

[0044] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention.

[0045] Unless otherwise defined, the technical or scientific terms used herein shall have the ordinary meaning as understood by one of ordinary skill in the art to which this invention pertains.

[0046] Example 1:

[0047] like Figures 1-6 As shown, this invention provides a path planning method for a snake robot based on fuzzy collision constraints and spiral trajectories, comprising the following steps:

[0048] Assume the system consists of It consists of nodes, with the first node being the head node and the rest being subsequent nodes. Each node has a length of . Width is The distance between nodes is The distance from the front end of the previous node is The distance from the last node to the tail is .

[0049] S1. Establish the overall path model of a multi-node series body using the Archimedes spiral equation;

[0050] S2. Solve the motion differential equation of the first node and recursively deduce the position of subsequent nodes to calculate the motion state of the entire system.

[0051] S3. Determine the collision risk between the first node and subsequent nodes using a fuzzy collision risk function to determine the safe movement boundary;

[0052] S4. Using pitch as the optimization variable, simulated annealing algorithm is used to optimize the path to adapt to spatial constraints under the premise of no collision.

[0053] Specifically, step S1, motion trajectory modeling:

[0054] Spiral path from the first The initial polar angle is [value missing]. Set the pitch to Then the coefficient of variation of the helix is:

[0055]

[0056] The system consists of It consists of nodes connected in series. The polar coordinate equation of the Archimedean spiral is as follows:

[0057]

[0058] The Cartesian coordinate equation of the Archimedean spiral is as follows:

[0059]

[0060] Specifically, step S2, node state calculation:

[0061] Set the head movement speed to The centers of all nodes of the snake-like robot lie on equidistant spirals. To establish the relationship between the rotation angle of the front node and time, the arc length formula in polar coordinates is introduced when the curve changes slightly:

[0062]

[0063] Substituting the extreme radius formula and its derivative into the simplified form, we get the following:

[0064]

[0065] The forward velocity of the front node of the head is maintained at Therefore, we can obtain .

[0066] On both sides of equation (5) Differentiating, we get:

[0067]

[0068] Furthermore, as the head spirals clockwise from the outside in along the Archimedean spiral, it gradually approaches the center point of the spiral, and the rotation angle changes from the initial value. Gradually decrease, therefore The change here is taken as negative, so the differential equation for the change of the head anterior node rotation angle with time is:

[0069]

[0070] In summary, the formula for the coordinates of the front node of the snake robot's head is:

[0071]

[0072] The system consists of It consists of 12 nodes, with the first node being the head node and the rest being subsequent nodes. The distance between the two head nodes is 1. The spacing between the remaining nodes is Based on the node spacing and the polar angle corresponding to the node in front of the head, the positions and corresponding polar angles of the remaining nodes can be deduced sequentially.

[0073] Utilizing spacing The formula for calculating the coordinates of the head node is as follows:

[0074]

[0075] The coordinates of each node on the snake's body and tail are determined using spacing. The formula for solving the coordinates is as follows:

[0076]

[0077] The formula for the fourth-order Runge-Kutta method is as follows:

[0078]

[0079] in For the current time step The value at that location, For the next time step The value at that location, The time step is set to 1 second.

[0080] The differential equation for the change of the rotation angle of the anterior node of the head over time is:

[0081]

[0082] Calculate the corresponding The middle slope The calculation formula is as follows:

[0083]

[0084] This allows us to solve for the polar angles corresponding to each time point of the anterior head node. By combining this with the spiral equation and transforming it into a Cartesian coordinate equation, we can then determine the anterior head node. The coordinates of each time point within the time frame, such as Figure 2 As shown.

[0085] Given the distance between the two head nodes Spacing between nodes of each segment of the snake's body Taking the node before the head as the first node, and based on the geometric constraint equations in S2.2, the second node (the node after the head), the third node, and so on, are solved sequentially using numerical methods. The position of the node (snake tail) at each moment. The movement trajectories of multiple nodes at different moments are as follows: Figure 3 As shown.

[0086] Specifically, step S3, collision risk assessment:

[0087] As the snake-like robot continues to coil, its head gradually approaches the center of the spiral, the radius decreases, and the curvature gradually increases. The angle between the nodes left between the inner and outer spirals becomes smaller and smaller, reducing the number of segments that can be accommodated on the same spiral. Therefore, when the head coils in to a certain extent, it will collide with the outer spiral body, at which point it can no longer coil in.

[0088] To achieve soft decision-making, a fuzzy risk function method is adopted, combined with geometric calculation of the coordinates of the apex of the board.

[0089] The coordinates of the first node before the first node are The tip of the board is located in front of the node, at a distance of:

[0090]

[0091] The diagonal angle is:

[0092]

[0093] The coordinates of the tip of the board are:

[0094]

[0095] For any subsequent node (the first...) (section), let the coordinates of its two nodes be... and The midpoint is:

[0096]

[0097] Euclidean distance from the cusp to the midpoint:

[0098]

[0099] Constructing a fuzzy risk function:

[0100]

[0101] when If the system is at a potential collision risk, the system will terminate the disk insertion. In this embodiment, the multi-node motion trajectory at the moment of disk insertion termination is as follows: Figure 5 As shown.

[0102] Specifically, step S4, path parameter optimization:

[0103] Assuming it needs to be in radius of The goal is to complete the turn within a circular area. The objective is to ensure that the leading edge of the head enters the defined turning space along the spiral without a collision, finding the minimum spiral pitch that satisfies the given conditions. .

[0104] In a Cartesian coordinate system with the pitch center as the origin, the equation of the circular region that defines the turning space is as follows:

[0105]

[0106] The simulated annealing algorithm is used for pitch optimization, with the following initial pitch: Initial temperature: Cooling factor: Standard deviation of disturbance: .

[0107] Disturbance generation:

[0108]

[0109] Objective function:

[0110]

[0111] in: Maximum fuzzy collision risk : The Euclidean distance between the endpoint and the boundary of the target circle.

[0112] Acceptance criteria: If Accept the new solution; otherwise, accept the solution based on probability. Accept suboptimal solution:

[0113]

[0114] Temperature update:

[0115]

[0116] In this embodiment, within a confined turning space, the process iterates until a termination condition is met, ensuring no collisions occur. The final output is the pitch corresponding to the minimum objective function. And used to generate the optimal collision-free path, such as Figure 6 As shown in the diagram, the path corresponding to this pitch is the optimal path that can safely reach the boundary within the defined space.

[0117] Through the complete demonstration of the above embodiments, it can be seen that the proposed snake robot path planning method based on fuzzy collision constraints and spiral trajectory is not only logically clear and the steps are well-defined, but also fully programmable and can efficiently and reliably solve the multi-node motion trajectory planning problem in complex environments.

[0118] Example 2:

[0119] The computer-readable storage medium of this embodiment stores a computer program that, when executed by a processor, implements the steps in the snake robot path planning method based on fuzzy collision constraints and helical trajectories of Embodiment 1.

[0120] The computer-readable storage medium in this embodiment can be an internal storage unit of the terminal, such as the terminal's hard disk or memory; the computer-readable storage medium in this embodiment can also be an external storage device of the terminal, such as a plug-in hard disk, smart memory card, secure digital card, flash memory card, etc. equipped on the terminal; furthermore, the computer-readable storage medium can include both the terminal's internal storage unit and external storage devices.

[0121] The computer-readable storage medium of this embodiment is used to store computer programs and other programs and data required by the terminal. The computer-readable storage medium can also be used to temporarily store data that has been output or will be output.

[0122] Example 3:

[0123] The computer device of this embodiment includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps in the snake robot path planning method based on fuzzy collision constraints and spiral trajectory of Embodiment 1.

[0124] In this embodiment, the processor can be a central processing unit, or other general-purpose processors, digital signal processors, application-specific integrated circuits, off-the-shelf programmable gate arrays or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor, etc. The memory can include read-only memory and random access memory, and provides instructions and data to the processor. A portion of the memory can also include non-volatile random access memory. For example, the memory can also store device type information.

[0125] Those skilled in the art will understand that the content disclosed in the embodiments can be provided as a method, system, or computer program product. Therefore, this solution can take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this solution can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage) containing computer-usable program code.

[0126] This solution is described with reference to flowchart illustrations and / or block diagrams of methods and computer program products according to embodiments of this solution. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing device, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0127] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0128] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0129] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0130] The examples described herein are merely preferred embodiments of the invention and are not intended to limit the concept and scope of the invention. Any modifications and improvements made by those skilled in the art to the technical solutions of the invention without departing from the design concept of the invention should fall within the protection scope of the invention.

[0131] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the specific embodiments described above. The specific embodiments and descriptions in the specification are merely for further illustrating the principles of the invention. Various changes and modifications can be made to the present invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the claims and their equivalents.

Claims

1. A path planning method for a snake robot based on fuzzy collision constraints and spiral trajectories, characterized in that, The system includes the following steps: Assume the system consists of... It consists of nodes, with the first node being the head node and the rest being subsequent nodes. Each node has a length of . Width is The distance between nodes is The distance from the front end of the previous node is The distance from the last node to the tail is ; S1. Motion trajectory modeling: Establish the overall motion path model of a multi-node connected body based on the polar coordinate equation of the Archimedes spiral; S2, Node State Calculation: Set the first node to move along the path, obtain the motion state of the first node by solving the motion differential equation, and recursively solve the position sequence of all subsequent nodes in the motion process based on the fixed distance between adjacent nodes; S3. Collision Risk Determination: By calculating the fuzzy distance relationship between the first node's tip and the rigid body line segment formed by subsequent nodes, a collision risk function is constructed, and whether the collision risk function exceeds a set threshold is used as the collision determination condition to determine the termination time of safe movement. S4. Path parameter optimization: Using the pitch of the Archimedean spiral as the optimization variable, a multi-objective evaluation function that integrates the collision risk function and the target area distance error is constructed. The simulated annealing algorithm is used for global optimization to obtain the minimum pitch value that satisfies the collision avoidance and spatial boundary conditions.

2. The method as described in claim 1, characterized in that, In step S1, the equation of the Archimedean spiral is as follows: Wherein, the polar diameter of the helix is The initial polar diameter of the helix is The coefficient of variation of the polar radius per unit rotation angle is The angles corresponding to each node on the spiral are The initial angle of rotation is .

3. The method as described in claim 2, characterized in that, In step S2, the differential equation of motion of the first node is derived through the polar coordinate arc length formula and the constant linear velocity condition, specifically as follows: in Let be the linear velocity of the first node moving along the spiral, and then solve it using the numerical integration method.

4. The method as described in claim 3, characterized in that, The recursive solution for the position of subsequent nodes in step S2 is achieved by solving the following formula: in, The distance between adjacent nodes. This is the Archimedes spiral equation.

5. The method as described in claim 4, characterized in that, In step S3, the coordinates of the first node's apex are determined by the coordinates of the preceding node, the node width, and the front offset. Subsequent nodes form rigid body segments by establishing the positions of the two nodes. The collision risk function is: in, The first cusp and the first The distance between the midpoints of the nodes; For sensing width parameters.

6. The method as described in claim 5, characterized in that, The simulated annealing algorithm described in step S4 includes the following steps: Initialize pitch parameters With temperature parameters ; Candidate solutions are generated using normal perturbations: Construct the objective function: in To minimize collision risk, The Euclidean distance from the endpoint of the first node to the target circular region; The new solution will be accepted with the following probabilities: Temperature update rules: The final output satisfies the minimum pitch .

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by the processor, it implements the steps in the snake robot path planning method based on fuzzy collision constraints and spiral trajectory as described in any one of claims 1 to 6.

8. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the snake robot path planning method based on fuzzy collision constraints and spiral trajectory as described in any one of claims 1 to 6.

Images