A Multi-Objective Point Path Planning Method for a Snake-like Robotic Arm

Abstract

This invention discloses a multi-objective point path planning method for snake-like robotic arms, comprising: initial path planning: reading the starting point and target point information, introducing random bias probability and heuristic function to improve the RRT algorithm, using planning speed and path node cost as evaluation indicators to generate the optimal path between points; path similarity analysis: introducing a cosine similarity calculation formula, starting from the starting point, iteratively calculating the path segment similarity with a set step size until the maximum similarity threshold is reached or the path iteration ends; path collision detection to obtain the transferable region of the path and the intermediate point between paths; multi-objective point fusion and selection: determining two postures of the snake-like robotic arm, using interpolation to fit the posture of the transfer process and the end transition motion path, fusing the current path, target path, etc. to form a multi-objective path; selecting the path with the smaller objective function as the working path of the snake-like robotic arm. This invention can be applied to the multi-objective point path planning problem of snake-like robotic arms.

Description

Technical Field

[0001] This invention relates to the field of robot path planning, specifically to a multi-objective point path planning method for snake-like robotic arms. Background Technology

[0002] In complex environments, confined spaces, and scenarios inaccessible to humans, such as high temperature and pressure, nuclear power plant detection, toxic waste sampling, large ship maintenance, and precision equipment maintenance, where the environment is complex and dangerous and people and large equipment cannot enter, the slender and flexible snake-shaped robotic arm has become an important choice due to its multiple degrees of freedom, high length-to-diameter ratio, lightweight structure, and high load capacity.

[0003] The high degree of redundancy in snake-like robotic arms grants them high dexterity, but also makes their motion planning problem extremely complex. Current research on path planning for snake-like robotic arms focuses on finding the optimal path between two points. However, real-world tasks typically involve multiple target work points, making existing path planning algorithms insufficient for meeting real-world task requirements. In conclusion, snake-like robotic arms have strong application prospects in various fields, and researching the multi-target point motion planning problem for snake-like robotic arms has significant practical implications.

[0004] Based on this, the present invention provides a multi-target point path planning method for snake-shaped robotic arms. Summary of the Invention

[0005] To address the multi-target point path planning problem of snake-like robotic arms in unstructured and complex environments, this invention proposes a novel multi-target point path planning method suitable for snake-like robotic arms, aiming to shorten the operation time of snake-like robotic arms and improve the stability of the working process.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] The initial path planning module is used to plan the initial path of the snake-like robotic arm in the task, providing a feasible solution for the work;

[0008] The path similarity analysis module is used to describe the similarity of spatial paths and to obtain transit points and transitional areas between paths.

[0009] The multi-target path fusion and selection module is used to fit transition paths from intermediate points between paths based on the snake-like robotic arm model, and select the optimal path to multiple target points according to the objective function.

[0010] Optionally, the initial path planning module specifically includes: The starting and target point information is read from the matrix, and the traditional RRT algorithm is improved by introducing random bias probability and heuristic function. The optimal path between each point is generated using planning speed and path node cost as evaluation indicators.

[0011] Optionally, the path similarity analysis module specifically includes: Path similarity calculation measures the degree of difference between two paths. A cosine similarity formula is introduced, and the similarity of path segments is calculated iteratively starting from the initial point with a set step size until the maximum similarity threshold is reached or the path iteration ends. Collision detection between paths is achieved by establishing a collision detection algorithm based on the spatial position of the serpentine robotic arm and obstacles. During the similarity calculation process, the two path vectors are connected, and the obstacle-free region between the two paths is obtained through collision detection. The largest obstacle-free region within the similarity range is the transferable region of the serpentine robotic arm's path, and its intersection with the boundary of the two paths is the transfer point between the paths.

[0012] Optionally, the multi-target path fusion and selection module specifically includes: Two postures of the snake-like robotic arm are determined based on the transfer point and the transferable area of ​​the path. The posture of the path transfer process and the end transition motion path are fitted by interpolation. The current path, target path, backtracking path and transition path are fused to form a multi-objective path. The effectiveness of path fusion is judged by the objective function, and the path with the smaller objective function is selected as the working path of the snake-like robotic arm.

[0013] Furthermore, the method for improving the RRT algorithm by introducing heuristic functions in the initial path planning module of this invention is as follows: The RRT algorithm is used to generate a fast exploration random tree, forming a series of feasible path points. The planned feasible points are then traversed and searched for all path points using the cost function of the improved A* algorithm. Cost estimation and selection are performed to obtain the minimum cost path while satisfying the joint angle constraints of the robotic arm.

[0014] Furthermore, the method for calculating the similarity between paths in the path similarity analysis module of the present invention is as follows: Starting from the origin, the algorithm expands along the path with a specific step size to obtain calculation points and form corresponding path direction vectors. The similarity between path segments is determined by calculating the cosine value between the path vectors. A new calculation point is obtained by expanding along the corresponding path with the original calculation point by one step size. This expansion and calculation is repeated until the end point of the path is reached or the vector cosine value exceeds a threshold.

[0015] The beneficial effects of this invention are: This invention addresses the multi-target point path planning problem of serpentine robotic arms. It utilizes principles such as the fast exploratory random tree method, the heuristic A* algorithm, and cosine similarity theory to design a multi-target point path planning method suitable for spatial serpentine robotic arms. Under the condition of satisfying the special constraints of the robotic arm mechanism, the serpentine robotic arm can generate and select path plans between the starting point and multiple target points according to actual needs in different unstructured environments, thereby reducing the running time of the serpentine robotic arm, reducing the working path distance, and enabling the serpentine robotic arm to work efficiently and stably. Attached Figure Description

[0016] The accompanying drawings, which form part of this invention, are used to provide a further understanding of this application. The illustrative embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation of this invention.

[0017] Figure 1 This is the overall system framework diagram of the present invention;

[0018] Figure 2 This is the overall system flowchart of the present invention;

[0019] Figure 3 This is a schematic diagram of the single-point planning result of the RRT-A* algorithm in this invention;

[0020] Figure 4 This is a schematic diagram of path similarity calculation in this invention;

[0021] Figure 5 This is a schematic diagram of the positional relationship for collision detection in this invention;

[0022] Figure 6 This is a diagram showing the planning results of the multi-objective point path planning of this invention; Detailed Implementation

[0023] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. It should be noted that the aspects described below with reference to the accompanying drawings and specific embodiments are merely exemplary and should not be construed as limiting the scope of protection of the present invention in any way.

[0024] Figure 1 This is a system block diagram of the multi-objective point path planning algorithm of the present invention. Figure 2 This is a flowchart illustrating the overall multi-objective point path planning algorithm of the present invention. The specific steps are as follows:

[0025] Step 1: Obtain the starting point and all target points during the snake-like robotic arm's task, and number all target points as P. T =[P T，1 P T，2 , ..., P T，n ] T ;where P TLet P be the target point matrix, n be the number of target points, and P be the target point matrix. T，1 P T，2 , ..., P T，n These represent the coordinates of target point 1 to n respectively;

[0026] Step 2: Using the RRT-A* algorithm, sequentially plan a path Path0 that starts from the starting point and can traverse all target points. Then, compare the path length L0 with the length L of the snake-shaped robotic arm segment. C Determine path feasibility; if L0 < L C If the path is feasible, the planning ends, and path Path0 is selected as the PATH for the snake-like robotic arm's motion path; if it is not feasible, proceed to step 3. The specific steps of the RRT-A* algorithm are as follows: Utilizing the traditional RRT algorithm with restrictions on the random tree expansion angle, and incorporating the target point bias probability. Initialize a random tree. Given an initial point q within the known working environment space. start Target point q goal And the search step size L, with q start As the root node of the exploration random tree; a sampling point q is randomly generated within the search space. rand And with a probability of 0.3, q goal Let q be the value of q. rand Search for distance q on a random tree rand The nearest node q near , q near As a new starting point, from q near With step size L towards q rand Expanding in the direction, we obtain a new node, denoted as q. new ; q near With q new Connect the elements and check if the connection path overlaps with any obstacles. If it does, the expansion is invalid and the expansion is repeated; otherwise, q is... new The node is added to the random tree, and then q... new Starting from q and along rand Expand the direction again. Repeat the above process until q is reached. goal After generating feasible paths, a heuristic A* cost function is introduced to estimate the cost of each feasible path point, constructing a cost matrix F. The improved path point cost calculation function is as follows: f(m)=[h(m)+g(m)]·Index, m≥2 (1) Where f(m) is the total cost of node m, g(m) is the cost from the initial point to node m, h(m) is the cost from node m to the target point, and Index is the feasibility coefficient of the node. Index = 1 when the node will not cause the path to exceed the angle limit; otherwise, Index = ∞. K1 and K2 are the direction vectors of the path segments, K... m Let K be the coordinates of node m. m-1 Let X be the coordinates of node m-1, and X be the unit vector of the robot arm base's forward direction. When estimating the node cost, the size of the optimized path turning angle is considered, prioritizing the satisfaction of the serpentine arm joint angle constraint; secondly, the node costs are compared, and nodes are selected sequentially starting from the starting point, with the lowest cost collision-free node being chosen as the next path point, ultimately obtaining the minimum cost path;

[0027] Step 3: Set the RRT-A* planning algorithm parameters and generate a program starting from point P. s To the target point P T，1 A feasible path Path1 is generated; a direct path from Path1 to the target point P is generated. T，2 The path is determined, and the feasibility of the entire path L is assessed. If the path L is less than the length L of the snake-shaped robotic arm segment... C If the path is valid, then the path is feasible; if feasible, retain the path segment and connect Path1 with this path to form Path2. The snake-like robotic arm can then pass through P in one go via Path2. T，1 With P T，2 Furthermore, no rollback operation or PATH update is required; if the path is unreachable, discard the path segment, do not update PATH, and continue planning.

[0028] Step 4: Use the RRT-A* algorithm to plan a feasible path from the starting point to the first unreachable target point in the list. Compare this path with the currently retained paths to analyze the differences between the two single-target point paths. Taking the forward direction of the serpentine robotic arm as the positive direction and the shortest path endpoint as the upper limit of the range, calculate the path similarity of path segments starting from the starting point and moving along the positive direction with a set step size. The method for calculating the similarity between paths is as follows: Starting from the starting point, extend along the path with a specific step size to obtain calculation points and form corresponding path direction vectors. Determine the similarity between path segments by calculating the cosine value between the path vectors. Extend the corresponding path with the original calculation point by one step size to obtain a new calculation point. Repeat the extension and calculation until the path endpoint is reached or the vector cosine value exceeds the threshold. The formula for calculating the cosine similarity between paths is: Among them A, A * These are the direction vectors from the starting point to the boundary points of the two path segments, respectively. A threshold for cosine similarity is set. Calculation stops immediately when the similarity function value falls below the preset threshold. Two path points not exceeding the threshold are the transition points of the multi-objective path. The maximum joint rotation angle θ of the snake-like robotic arm is used as the basis for this calculation. max Adjust the threshold value for cosine similarity; cosθ max The set threshold; A collision detection algorithm is established by using the spherical envelope detection method to determine the spatial position of the snake-like robotic arm and obstacles. The algorithm detects the presence of obstacles between the two paths sequentially from the starting point with the step size mentioned above. Through collision detection calculation, the maximum obstacle-free area of ​​the snake-like robotic arm between the two paths is determined. A collision detection algorithm is established by determining the spatial position of the snake-like robotic arm relative to obstacles, such as... Figure 4 As shown, the presence of obstacles between the two paths is detected sequentially from the starting point using the aforementioned step size. The maximum obstacle-free area between the two paths is determined by calculating using a collision detection algorithm.

[0029] Step 5: Based on cosine similarity and the obstacle-free area between paths, obtain the path transfer area and path transfer points for the snake-like robotic arm to move between two paths without collision; determine the discard path, the backtracking path of the first path, the retained path of the second path, and the posture of the snake-like robotic arm at the two transfer points based on the position of the transfer points; calculate the joint angles of the snake-like robotic arm in the two states using the mapping relationship from task space to joint space; obtain the joint angle sequence during the path transfer process of the snake-like robotic arm using cubic spline interpolation; and obtain the end motion trajectory of the transition segment through kinematic analysis. Integrate the backtracking path, the transition path, and the forward retained path to obtain a multi-target point motion path suitable for the snake-like robotic arm. At this point, from the starting point P s Reaching two target points P a P b There are two methods: (1) by P s via Path a Arrival at P a The robotic arm retracted to P s Then from the starting point through Path b Arrival at P b (2) By P s via Path a Arrival at P a The robotic arm retracts to the transfer point and transitions to the Path via the transferable area. b Then through Path b Arrival at P b .

[0030] Step 6: Evaluate and compare the sum of the path costs of the two independent paths in Step 4 with the path cost of the merged human path in Step 5, i.e., the D values ​​of the two schemes described in Step 5. Retain the scheme with the lower cost as the actual motion path of the snake-like robotic arm and update it in the PATH. Define an optimization parameter D to evaluate the path cost and determine the effectiveness of the planned path. The function expression of D is: D=ω t D t +ω l D l (5) Among them, D t D represents the movement time of the snake-like robotic arm. l ω represents the length of the serpentine mechanical motion path. t ω l It is D t D l The weighting coefficient, ω t =0.67, ω l =0.33; the smaller the D value, the better the corresponding path performs;

[0031] Step 7: Plan the path along the latest reserved path i Directly reach the target point P T，i+1 The path is determined, and the feasibility of the entire path L is assessed. If the path L is less than the length L of the snake-shaped robotic arm segment... C If the path is valid, then the path is feasible; if feasible, retain the path segment and combine it with the Path. i Forming a Path i+1 Update PATH. If the path is unreachable, discard the path segment and do not update PATH. Proceed to step 7. Repeat the above operations until all target points have been traversed.

[0032] like Figure 5 As shown, a simple multi-obstacle environment and two target points are set up to illustrate the basic idea of ​​the multi-target point path planning algorithm proposed in this invention. Figure 5 As shown in (a) and (b), the starting point P is set. S Target point P T，1 Target point 2 P T，2 The position parameters are planned according to the multi-target point path planning method for the snake-like robotic arm proposed in this invention. First, a path is planned from P S →P T，1 →P T，2 If a path is selected that can reach all target points without backtracking, and the path length meets the constraints and the path turning angle does not exceed the mechanism constraints, then this path will be selected as the motion path of the snake-like robotic arm. When the location of the target point changes, such as Figure 5As shown in (c) and (d), a continuous path reaching both target points and satisfying the length limit cannot be obtained. Therefore, single-point planning is performed twice for the starting point and the two target points. After obtaining two independent paths, the similarity between the two paths is calculated to obtain the transfer point. Starting from the starting point, along the positive direction of the serpentine robotic arm (i.e., the positive x-axis), the similarity is calculated segment by segment. Specifically, starting from the starting point, the similarity is expanded along both paths with a step length h to obtain calculation points. The starting point and the two calculation points are connected to form two direction vectors, and the cosine value between the vectors is calculated. If the cosine value does not exceed a set threshold, it is considered to satisfy the similarity. At the same time, collision detection is performed on the region between the starting point and the two calculation points. If there is no collision, the calculation can continue. Then, the two calculation points are expanded along the two paths with a step length to obtain two new calculation points. The above steps are repeated until the end point of the path is reached, or the vector cosine value exceeds the threshold, or there is a collision between regions. The transfer point between path 1 and path 2 is obtained according to the above method. After calculating D 融合 <(D) 路径1 +D 路径2 Therefore, the fusion path is selected as the motion path for the snake-like robotic arm. A schematic diagram of the snake-like robotic arm's motion path and sequence is shown below. Figure 5 As shown in (d).

[0033] like Figure 6 As shown, a narrow, obstacle-filled perforated plate structure was used as the experimental environment to implement multi-objective point path planning for a snake-like robotic arm. Figure 6 (a) is the RRT algorithm. Figure 6 (b) is the proposed RRT-A* optimization algorithm. Figure 6 (c) The RRT algorithm that utilizes the proposed multi-objective point planning algorithm. Figure 6 (d) The RRT-A* algorithm, based on the proposed multi-objective point planning algorithm, was applied. The planning results are shown in the table below:

[0034] The table shows the end-effector movement distance and base movement distance for four planning methods. It can be seen that, regardless of whether it's the RRT algorithm or the RRT-A* algorithm, the multi-objective-point planning approach proposed in this invention is superior to the traditional single-objective-point, multiple-planning motion method. The method of this invention can quickly generate and autonomously select motion paths suitable for the snake-like robotic arm based on the location of the target point. It can efficiently explore various unstructured environments, saving time required for path planning and shortening the planned path distance. In practical work, it effectively reduces the distance and energy consumption required to travel between various target points.

[0035] Although the method has been described step by step through illustrative specific embodiments of the invention so that those skilled in the art can understand it, the invention is not limited to the scope of the specific embodiments. Those skilled in the art can make various modifications or variations within the scope of the claims, as long as such variations are within the spirit and scope of the invention as defined and determined by the appended claims.

Claims

1. A multi-objective point path planning method for a snake-like robotic arm, characterized in that: This method comprises three modules: initial path planning, path similarity analysis, and multi-objective path fusion and selection. The initial path planning module plans the initial path of the snake-like robotic arm in the task, providing a feasible solution for the work. The path similarity analysis module describes the similarity of spatial paths, obtains the intermediate points and transitional regions between paths, and the multi-objective path fusion and selection module fits transitional paths from the intermediate points between paths based on the snake-like robotic arm model, and selects the optimal path to multiple target points according to the objective function. The specific steps of the initial path planning algorithm are as follows: Step 1: Obtain the starting point and all target points during the snake-like robotic arm's task, and number all target points as P. T =[P T，1 P T，2 , ..., P T，n ] T ;where P T Let P be the target point matrix, n be the number of target points, and P be the target point matrix. T，1 P T，2 , ..., P T，n These represent the coordinates of target point 1 to n respectively; Step 2: Plan a path Path0 that starts from the starting point and can traverse all target points in sequence. Then, compare the path length L0 with the length L of the snake-shaped robotic arm segment. C Determine path feasibility; if L0 < L C If the path is feasible, the planning ends, and path Path0 is selected as the PATH for the snake-like robotic arm's motion path; if it is not feasible, proceed to step 3. Step 3: Initialize the RRT-A* planning algorithm parameters and generate a starting point P. S To the target point P T，1 A feasible path Path1 is generated; a direct path from Path1 to the target point P is generated. T，2 The path is determined, and the feasibility of the entire path L is assessed. If the path L is less than the length L of the snake-shaped robotic arm segment... C If the path is valid, then the path is feasible; if feasible, retain the path segment and connect Path1 with this path to form Path2. The snake-like robotic arm can then pass through P in one go via Path2. T，1 With P T，2 Furthermore, no rollback operation or PATH update is required; if the path is unreachable, discard the path segment, do not update PATH, and continue planning. Step 4: Use the RRT-A* algorithm to plan a feasible path from the starting point to the first unreachable target point in the list, compare it with the currently retained path, analyze the path difference between the two single target point paths, take the forward direction of the snake-like robotic arm as the positive direction, take the shortest path endpoint as the upper limit of the range, and calculate the path similarity of the path segment along the positive direction from the starting point with a set step size. A collision detection algorithm is established by using the spherical envelope detection method to determine the spatial position of the snake-like robotic arm and obstacles. The algorithm detects the presence of obstacles between the two paths sequentially from the starting point with the step size mentioned above. Through collision detection calculation, the maximum obstacle-free area of ​​the snake-like robotic arm between the two paths is determined. Step 5: Based on cosine similarity and the obstacle-free area between paths, obtain the path transfer area and path transfer point for the snake-like robot arm to transfer between two paths without collision; determine the discard path, the backtracking path of the first path, the retained path of the second path, and the posture of the snake-like robot arm at the two transfer points based on the position of the transfer point; calculate the joint angles of the snake-like robot arm in the two states respectively by the mapping relationship from the task space to the joint space; obtain the joint angle sequence during the path transfer process of the snake-like robot arm using cubic spline interpolation; obtain the motion trajectory at the end of the transition segment through kinematic analysis; fuse the backtracking path, the transition path, and the forward retained path to obtain a multi-target point motion path suitable for the snake-like robot arm; Step 6: Evaluate and compare the sum of the path costs of the two independent paths in Step 4 with the path cost of the merged human path in Step 5, i.e., the D values ​​of the two schemes described in Step 5. Retain the scheme with the lower cost as the actual motion path of the snake-like robotic arm and update it in the PATH. Define an optimization parameter D to evaluate the path cost and determine the effectiveness of the planned path. The function expression of D is: D=ω t D t +oh l D l (1) Among them, D t D represents the movement time of the snake-like robotic arm. l ω represents the length of the serpentine mechanical motion path. t ω l It is D t D l The weighting coefficient, ω t =0.67, ω l =0.33; the smaller the D value, the better the corresponding path performs; Step 7: Plan the path along the latest reserved path i Directly reach the target point P T，i+1 The path is determined, and the feasibility of the entire path L is assessed. If the path L is less than the length L of the snake-shaped robotic arm segment... C If the path is valid, then the path is feasible; if feasible, retain the path segment and combine it with the Path. i Forming a Path i+1 Update PATH. If the path is unreachable, discard the path segment and do not update PATH. Proceed to step 7. Repeat the above operations until all target points have been traversed.

2. The initial path planning module of the multi-objective point path planning method for a snake-like robotic arm according to claim 1, its core lies in: after forming feasible path points, introducing an A* heuristic cost function to find the minimum cost path while satisfying the joint angle constraints of the robotic arm; wherein the improved path point cost calculation function is: f(m)=[h(m)+g(m)]·Index, m≥2 (2) Where f(m) is the total cost of node m, g(m) is the cost from the initial point to node m, h(m) is the cost from node m to the target point, and Index is the feasibility coefficient of the node. Index = 1 when the node will not cause the path to exceed the angle limit; otherwise, Index = ∞. K1 and K2 are the direction vectors of the path segments, K... m Let K be the coordinates of node m. m-1 Let X be the coordinates of node m-1, and let X be the unit vector of the forward direction of the robotic arm base.

3. The core of the multi-objective point path planning method for a snake-like robotic arm according to claim 1 is: The method for calculating path similarity in the path similarity analysis module is as follows: Starting from the starting point, the calculation points are obtained by expanding along the path with a specific step size, and corresponding path direction vectors are formed. The similarity between path segments is determined by calculating the cosine value between the path vectors. The original calculation point is extended along the corresponding path by one step to obtain a new calculation point. This extension and calculation are repeated until the end of the path is reached or the vector cosine value exceeds the threshold. The formula for calculating the cosine similarity between paths is: Among them A, A * These are the direction vectors from the starting point to the boundary points of the two path segments, respectively. A threshold for cosine similarity is set. Calculation stops immediately when the similarity function value falls below the preset threshold. Two path points not exceeding the threshold are the transition points of the multi-objective path. The maximum joint rotation angle θ of the snake-like robotic arm is used as the basis for this calculation. max Adjust the threshold value for cosine similarity; cosθ max The threshold value is set.

Images