Environmental perception mechanical arm path planning method and device facing masonry task

Abstract

The invention provides an environment perception mechanical arm path planning method and device for a masonry task, and the method comprises the following steps: building an environment perception mechanical arm path planning model which is based on an RRT * algorithm employing a bidirectional expansion strategy, and employs a pre-distribution expansion node strategy and an environment perception mechanism to update new nodes of the RRT * algorithm; the obtained mechanical arm kinematics model, the environment data and the starting point and the target point of mechanical arm motion are input into an environment sensing mechanical arm path planning model to obtain an initial path; performing redundant point removal and path smoothing operation on the initial path based on the mechanical arm pose constraint to obtain a mechanical arm motion path facing the masonry task; and the mechanical arm carries out a masonry task by using the obtained mechanical arm motion path, the environment data is updated through a depth camera sensing mechanism in the masonry task proceeding process, the steps are repeated, and the mechanical arm motion path is updated. The method provides guarantee for operation of the mechanical arm in a masonry task.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular to a method and device for path planning of an environment-aware robotic arm for masonry tasks. Background Art

[0002] In the field of robotic arm path planning, the RRT (rapid random tree) algorithm has been widely used and achieved many research results due to its probabilistic completeness and good search ability in complex high-dimensional space. However, with the continuous deepening of robotic arms in practical application scenarios such as masonry tasks, the existing RRT improved algorithm has exposed obvious limitations and is difficult to meet the actual needs in complex construction scenarios. A series of key technical problems need to be solved urgently.

[0003] Most of the existing RRT improvement algorithms focus on improving a single performance indicator, especially the convergence speed. By optimizing the sampling strategy and introducing heuristic information, the speed at which the algorithm finds a feasible path has indeed been accelerated to a certain extent. However, in the execution of masonry tasks, it is far from enough to simply pursue fast convergence. The robot needs to find an obstacle avoidance path that is not only feasible but also of high quality in complex building scenes. Fast convergence may mean that the algorithm falls into a local optimal solution too early, and the planned path may have too many redundant nodes, unreasonable turning angles, or too long a path length. This will not only increase the motion energy consumption of the robot, but may also affect the execution efficiency and accuracy of the masonry task. Therefore, how to ensure the convergence speed of the algorithm while taking into account the path quality and finding a path with a shorter length, smoother turning, and more in line with the motion characteristics of the robot is the primary technical problem that needs to be solved.

[0004] In complex construction scenes, the distribution and shapes of obstacles are complex and diverse, including walls, building materials, and other construction equipment. Existing RRT algorithms have obvious deficiencies in obstacle perception. They usually simply perform path planning based on known environmental maps and lack the ability to perceive and avoid dynamic obstacles in real time. During the execution of masonry tasks, the surrounding environment may change at any time, such as newly moved in building materials, moving construction workers, etc. If the robot arm cannot perceive these dynamic obstacles in time, it may cause a collision accident, which will not only damage the robot arm and surrounding objects, but also endanger the safety of construction workers. Therefore, how to enhance the algorithm's ability to perceive surrounding obstacles and achieve real-time dynamic obstacle avoidance is a key technical issue to ensure that the robot arm can safely and efficiently perform masonry tasks in complex construction scenes.

[0005] The working space and structural constraints of the robotic arm itself are important factors that must be considered in path planning. Different robotic arms have different working space ranges, joint movement ranges, and kinematic characteristics. When the existing RRT algorithms plan paths, they often do not fully consider these factors, resulting in the planned paths may exceed the working space of the robotic arm or cannot meet the structural constraints of the robotic arm. For example, some paths may require the joint angles of the robotic arm to exceed their physical limits, or singularities may occur during the movement, causing the robotic arm to be unable to perform actions normally. Therefore, how to combine the working space and structural constraints of the robotic arm itself, enhance the adaptability of the algorithm to the robotic arm, and plan a path that conforms to the motion characteristics of the robotic arm is an important technical issue to improve the execution efficiency and stability of the robotic arm in the masonry task.

[0006] In summary, in the robotic arm path planning for masonry tasks, the existing improved RRT algorithms have obvious deficiencies in terms of convergence speed, obstacle perception ability, and robotic arm adaptability. To solve these problems, it is necessary to comprehensively consider algorithm performance, environmental perception, and robotic arm characteristics, and develop a new path planning algorithm that can balance convergence speed and path quality, enhance obstacle perception ability, and adapt to the working space and structural constraints of the robotic arm. Summary of the Invention

[0007] To solve the problems existing in the prior art, the present invention provides an environmental perception robotic arm path planning method and device for masonry tasks, and performs multi-dimensional optimization on the traditional RRT* algorithm for the masonry task scenario. On the one hand, a pre-allocated expansion node strategy is proposed. After random sampling, candidate expansion points are pre-allocated for the vertices, new vertices are selected according to the proximity between the vertices and the random sampling points, and collision detection is performed. At the same time, dead vertices are removed, effectively improving the efficiency and rationality of node expansion. On the other hand, an environmental perception mechanism is introduced. Through local sampling and environmental classification, different environmental types can be accurately identified, and then expansion points can be reasonably selected to solve the influence of complex obstacles at the masonry construction site on the passability of the robotic arm. In addition, during the path planning process, path optimization is combined with the pose constraints of the robotic arm, and the environmental data is updated in real time using the depth camera perception mechanism to dynamically adjust the path, enhancing the environmental perception ability of the algorithm. While improving the planning efficiency and path quality of the algorithm, it ensures that the robotic arm can efficiently and safely complete the masonry task.

[0008] To achieve the above object, the present invention provides the following technical solution: An environmental perception robotic arm path planning method for masonry tasks, the specific steps are as follows:

[0009] S1. Establish an environmental perception robotic arm path planning model. The model is based on the RRT* algorithm using a bidirectional expansion strategy, and the pre-allocated expansion node strategy and environmental perception mechanism are used to update the new nodes of the RRT* algorithm;

[0010] S2. Input the obtained kinematic model of the robotic arm, environmental data, starting point and target point of the robotic arm movement into the environmental perception robotic arm path planning model to obtain an initial path;

[0011] S3. Based on the pose constraints of the robotic arm, perform operations of removing redundant points and path smoothing on the initial path to obtain the robotic arm movement path for the masonry task;

[0012] S4. The robotic arm uses the obtained robotic arm movement path to perform the masonry task. During the masonry task, update the environmental data through the depth camera perception mechanism, and repeat S1 - S3 to update the robotic arm movement path for the masonry task.

[0013] Furthermore, in S1, the specific steps of the pre - allocated expansion node strategy are as follows:

[0014] 1) Candidate expansion point setting

[0015] After the RRT* algorithm randomly samples x rand , select the nearest vertex x nearest as the starting point, and pre - allocate candidate expansion points sim nearest for the vertex x j , and set the angle between each vertex and its connection point to 120°;

[0016] 2) Select a new vertex

[0017] According to the proximity of the vertex x new to x rand , select the vertex x nearest from the candidate expansion points of x new ;

[0018] 3) Perform collision detection

[0019] If the boundary x new -x nearest does not collide with the obstacle, then x new is added as a new vertex to the random tree, and a group of candidate expansion points are pre - allocated for x new , and the candidate expansion points cannot overlap with any existing points in the tree;

[0020] If the boundary x new -x nearest collides with the obstacle, use the environmental perception mechanism to process the obstacle area;

[0021] When the candidate expansion point set of the vertex is empty, then classify it as a dead vertex and remove it.

[0022] Furthermore, in S1, the specific steps of the environmental perception mechanism are as follows:

[0023] 1) Local sampling

[0024] Using the extended step size as the radius, uniformly sample n points around x nearest and divide them into a set of obstacle area points, a set of free area points, and a set of boundary points between the obstacle area and the free area. The boundary points belong to the set of free area points;

[0025] The number of sampling points n must satisfy the following formula:

[0026]

[0027] where π is the angle, d Width is the channel width, and the minimum feasible channel width is obtained from the volume of the manipulator itself. d step is the extended step size, and n is the number of sampling points;

[0028] 2) Environment classification:

[0029] When the set of obstacle area points has only two vertices and the set of free area points has more than two vertices, the environment is a wall obstacle, and random sampling is performed again; otherwise, the environment is a complex obstacle area;

[0030] When the environment is a complex obstacle area, using the node x nearest as the fulcrum and the extended step size as the radius, divide it into multiple fan-shaped areas, and each fan-shaped area contains only a set of continuous W free points;

[0031] Using the fan-shaped area that does not contain the parent node x parent point as the extended area. In the case of a single-channel situation, select a W free point that will not cause a collision from the extended area as the new extended point x new ; in the case of a multi-channel branch situation, select multiple W free points that will not cause a collision from the extended area as the new extended point x new .

[0032] Furthermore, in S3, the steps for optimizing the initial path are specifically as follows:

[0033] Preliminary smoothing processing: Based on the manipulator pose constraints, remove redundant points and smooth the path of the initial path to obtain a smooth path;

[0034] Manipulator collision detection path smoothing processing: Based on the smooth path, judge the reachability of the manipulator link pose through the principle of inverse kinematics of the manipulator, and use the AABB envelope box model to judge whether the manipulator collides with obstacles to obtain a smooth path after collision detection;

[0035] Smoothing the path of the masonry task: Obtain the geometric center point of the i-th block and j-th layer of masonry in the masonry task, and use the distance between the geometric center point and the position vector at any moment in the robotic arm path to further perform collision detection between the robotic arm and obstacles, and obtain an optimal path where the pose of the robotic arm is reachable and there is no collision in the masonry task as the movement path of the robotic arm.

[0036] Further, in the path smoothing of the masonry task, a path risk cost function is established through the distance between the geometric center point and the position vector at any moment in the robotic arm path. The path risk cost function is as follows:

[0037]

[0038] Among them, the total path risk cost value L total takes values from 0 to 1, and the closer it is to zero, the lower the collision risk; P i,j represents the geometric center point of the i-th block and j-th layer of masonry, which is determined according to the masonry method; P arm (t) is the position vector at any moment in the robotic arm path, and σ is the collision sensitivity coefficient;

[0039] Normalize the total path risk cost value L total and define it as the average risk per unit path length:

[0040]

[0041] Among them, T is the path execution duration, that is, the total time experienced by the robotic arm from the starting point to the ending point; R ∈ (0, ∑ i,j 1];

[0042] Path safety assessment criteria:

[0043] R ≥ 0.5: High risk, the path needs to be re-planned;

[0044] 0.2 ≤ R < 0.5: Medium risk, there are areas close to obstacles in the path, and it should be optimized;

[0045] 0.05 ≤ R < 0.2: Low risk, executable;

[0046] R < 0.05: Extremely low risk, the path quality is optimal.

[0047] Further, in S4, the depth camera perception mechanism is specifically as follows:

[0048] Use the depth camera to obtain a dense depth image in the working space of the robotic arm, map it into a set of spatial voxels, and construct the local occupancy map information of the current frame;

[0049] The sliding window mechanism is adopted to maintain the local occupancy map information in real time. When it is detected that there is a newly placed masonry or the existing placed masonry has moved within the area covered by the original path, the environmental map is updated and the path feasibility is re-evaluated;

[0050] During the path expansion process, the voxels with occupancy probability exceeding the threshold are marked, and the corresponding expansion node candidate area in the pre-allocated expansion node strategy is dynamically adjusted;

[0051] The environmental map update process can be expressed as:

[0052]

[0053] Where: M t is the environmental map; is the occupancy probability inferred from the depth map of the current frame; α is the fusion weight; M t+1 is the updated map.

[0054] The present invention also provides an environmental perception robotic arm path planning system for masonry tasks, including:

[0055] A model establishment module, used to establish an environmental perception robotic arm path planning model, which is based on the RRT* algorithm adopting a bidirectional expansion strategy, and updates the new nodes of the RRT* algorithm by using a pre-allocated expansion node strategy and an environmental perception mechanism;

[0056] A path planning module, used to input the obtained robotic arm kinematic model, environmental data, starting point and target point of the robotic arm movement into the environmental perception robotic arm path planning model to obtain an initial path;

[0057] A path optimization module, used to perform redundant point removal and path smoothing operations on the initial path based on the robotic arm pose constraint to obtain the robotic arm movement path for masonry tasks;

[0058] A path update module, used for the robotic arm to perform masonry tasks using the obtained robotic arm movement path. During the masonry task, the environmental data is updated through the depth camera perception mechanism, and then the robotic arm movement path for masonry tasks is updated.

[0059] The present invention also provides a terminal device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the above-mentioned environmental perception robotic arm path planning method for masonry tasks are implemented.

[0060] The present invention also provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, the steps of the above-mentioned method for path planning of an environment-aware robotic arm for masonry tasks are implemented.

[0061] The present invention also provides a computer program product including a computer program, and when the computer program is executed by a processor, the steps of the above-mentioned method for path planning of an environment-aware robotic arm for masonry tasks are implemented.

[0062] Compared with the prior art, the present invention has at least the following beneficial effects:

[0063] A method for path planning of an environment-aware robotic arm for masonry tasks proposed by the present invention adopts a pre-allocated extended node strategy and combines an environment perception mechanism to update new nodes of the RRT* algorithm. This innovative measure effectively improves the expansion efficiency of nodes, enabling the algorithm to more quickly construct the random tree structure required for path planning. Through preset node traversal and the environment perception mechanism, nodes in unreachable regions can be accurately determined and node death processing can be performed, greatly improving the utilization rate of nodes. At the same time, the proposed bidirectional expansion strategy takes the starting point and the target point as the initial nodes of two random trees respectively. Under the perception bias guided by the target, the two random trees grow towards each other with a certain probability. This bidirectional expansion method speeds up the exploration of unknown regions, significantly improves the path planning efficiency, and can find a feasible path faster than the unidirectional expansion strategy, effectively solving the problem of difficult path planning in complex construction scenarios by traditional methods.

[0064] A method for path planning of an environment-aware robotic arm for masonry tasks proposed by the present invention not only focuses on the efficiency of path planning, but also deeply optimizes the generated initial path. By removing redundant points and performing path smoothing operations, removing redundant points can shorten the path length, reduce unnecessary movements of the robotic arm, and reduce energy consumption and movement time; path smoothing optimization further improves the path quality, making the movement of the robotic arm smoother and more efficient, and avoiding problems such as unstable movement of the robotic arm caused by a rough path. In addition, based on the principle of inverse kinematics of the robotic arm, the reachability of the robotic arm link pose is judged, and the envelope box model is used to judge whether the robotic arm collides with obstacles, so as to find an optimal path where the robotic arm pose is reachable and there is no collision. This mechanism ensures that the planned path is not only geometrically feasible, but also will not collide with obstacles during the actual movement of the robotic arm, improving the reliability and safety of path planning and ensuring the stable operation of the robotic arm in masonry tasks.

[0065] An environmental perception robotic arm path planning method proposed by the present invention updates environmental data in real time through a depth camera perception mechanism during the masonry task. The depth camera is used to obtain a dense depth image within the robotic arm's workspace, which is mapped into a spatial voxel set to construct the local occupancy map information of the current frame and is maintained in real time using a sliding window mechanism. When it is detected that there is a newly placed masonry or a movement of the originally placed masonry within the area covered by the original path, the environmental map is updated in a timely manner, and the path feasibility is re-evaluated. During the path expansion process, voxels with an occupancy probability exceeding the threshold are marked, and the corresponding expansion node candidate area in the pre-allocated expansion node strategy is dynamically adjusted.

[0066] The simulation experiment results show that the method of the present invention can still find an effective path executable by the robotic arm in a complex scenario, fully demonstrating the strong adaptability of the method to environmental changes and the high reliability of path planning, providing a strong guarantee for the efficient and safe operation of the robotic arm in the masonry task. Description of the Drawings

[0067] Figure 1 is the RRT expansion method;

[0068] Figure 2 is the RRT* expansion method;

[0069] Figure 3 is the structural flowchart of the CS-RRT* algorithm;

[0070] Figure 4 is the regional repeated search graph; (a) is the repeated search using the traditional biasing strategy; (b) is the improved biasing strategy;

[0071] Figure 5 is the improved vertex expansion process; (a) is before expansion; (b) is selecting the nearest X after sampling; (c) is after expansion;

[0072] Figure 6 is the environmental perception process;

[0073] Figure 7 is the complex area perception (a) the entrance of the obstacle area, (b) the interior of the obstacle area (c) the exit of the obstacle area (d) multiple branch obstacle areas;

[0074] Figure 8 is the schematic diagram of bidirectional expansion;

[0075] Figure 9 is the schematic diagram of pruning redundant nodes;

[0076] Figure 10 is the schematic diagram of the node position update process;

[0077] Figure 11 It is a schematic diagram of the path smoothing process;

[0078] Figure 12 It is a simplified model diagram of collision detection;

[0079] Figure 13 It is an environmental perception diagram of a depth camera;

[0080] Figure 14 It is a comparison diagram of path search of three algorithms in a simple two-dimensional scenario;

[0081] Figure 15 It is a comparison diagram of path search of three algorithms in a complex two-dimensional scenario;

[0082] Figure 16 It is a comparison diagram of path search of four algorithms in a three-dimensional scenario;

[0083] Figure 17 It is the result of robotic arm simulation path planning;

[0084] Figure 18 It is a robotic arm joint path diagram;

[0085] Figure 19 It is a robotic arm joint trajectory diagram;

[0086] Figure 20 It is a step-by-step diagram of robotic arm simulation grasping. Specific implementation manner

[0087] The present invention will be further described below in conjunction with the accompanying drawings and specific implementation manners.

[0088] The present invention proposes an environmental perception robotic arm path planning method for masonry tasks, including the following steps:

[0089] First, judge the reachability of the robotic arm link pose through the principle of robotic arm inverse kinematics, and consider the volume of the robotic arm itself during the subsequent algorithm search process, that is, use the bounding box model to judge whether the robotic arm collides with obstacles;

[0090] Secondly, design a preset node traversal and environmental perception mechanism, improve the utilization rate of nodes by determining unreachable area nodes and performing node death processing on them, and guide the rapidly-exploring random tree to quickly identify and smoothly pass through complex scenes such as masonry walls at the construction site;

[0091] Then, propose a two-way expansion strategy, use the starting point and the target point as the initial nodes of two random trees respectively, and under the target-guided perception bias, the two random trees grow towards each other with a certain probability to accelerate the exploration of unknown areas and improve the path planning efficiency;

[0092] Finally, pruning and redundancy removal are performed on the generated path to shorten the path length, and smoothing optimization is carried out on redundant nodes to further improve the path quality. Based on the smoothed path, the reachability of the manipulator link pose is judged by the inverse kinematics principle of the manipulator, and the AABB bounding box model is used to judge whether the manipulator collides with obstacles, and an optimal path with reachable and collision-free manipulator pose is obtained as the manipulator motion path for the masonry task.

[0093] The planning method is as follows:

[0094] 1. RRT* algorithm

[0095] The core principle of the Rapidly-exploring Random Tree (RRT) algorithm is to construct a tree-like path network in the configuration space through probabilistic sampling. As Figure 1 shown in the figure, the random tree in the figure generates new nodes by expanding and connecting nodes, and finally finds a path from the starting point to the target point.

[0096] First, a state point x rand is randomly sampled from the environment. This point serves as the root node, that is, the initial parent node, and the node x rand nearest to x near is found in the constructed tree;

[0097] Then, let x near grow a step length in the direction towards x rand , and a new node x new is generated at the end of the growth. If the path from x near to x new does not pass through obstacles, the x new node is added to the tree.

[0098] Repeat the above process until the target point x goal is reached, indicating that the path planning task is completed.

[0099] The RRT* algorithm is an improved algorithm of the RRT algorithm, and its schematic diagram is as Figure 2 shown. The process of the RRT* algorithm is basically the same as that of the RRT algorithm. The main differences are in two places:

[0100] The first is that the RRT* algorithm needs to reselect the parent node for x new to minimize the length of the path from x new to the starting point. Reselecting the parent node means taking the new node x new as the center and a defined length r as the radius to obtain a circular area, and searching for all neighboring nodes in the area as replacements for the original parent node x new x nearThe alternative nodes, and then calculate the path cost from the starting point to each neighboring node plus the path cost from the neighboring node to x new in turn, and select the neighboring node x with the minimum path cost min as the new parent node of x new

[0101] The second is that after reselecting the parent node, it is necessary to rewire all the neighboring nodes of this node. The wiring principle is to minimize the length of the path from all nodes to the starting point.

[0102] 2. Bi-directional Extended RRT* Algorithm Incorporating Environment Perception

[0103] 2.1 CS-RRT* Algorithm Structure

[0104] The process of the bi-directional extended RRT* algorithm incorporating environment perception (CS-RRT*) is as Figure 3 shown, and the implementation steps are as follows:

[0105] Step1: Initialize the map and related environment information, set the starting point and the target point, and use them as the root nodes of two random trees (denoted as Tree 1 and Tree 2) respectively. At the same time, enhance the environmental coverage through preset node traversal, initialize the path set and the smooth path set, and set control parameters such as the target bias probability threshold, the sampling distance threshold, and the step size.

[0106] Step2: The two random trees expand towards each other simultaneously. Combining the target bias strategy and the environment perception mechanism, a target point is selected as the guide with a certain probability to generate a random sampling point x rand , find the tree node x in the current tree that is closest to the random sampling point near , and generate a new node x along this direction new . If the generated x new does not meet the target area constraint or environmental limit, resample within the feasible area to generate a node that meets the conditions, and use it as the new node to be expanded.

[0107] Step3: Perform a collision detection on the newly generated node x new . If the detection passes, connect it to the tree structure and select the optimal parent node based on the principle of the minimum path cost. At the same time, rewire the nodes within its neighborhood to achieve local path optimization. If the detection fails, discard the node and return to Step 2.

[0108] Step4: In each iteration, detect the distance between the closest nodes in the two random trees. If this distance is less than the set connection threshold, it is considered that the two trees are successfully connected and the path search is completed; otherwise, continue the iterative expansion.

[0109] ​Step 5: Backtrack from the connecting nodes in the two trees to the starting point and the target point respectively to form a complete path. Subsequently, add this path to the path set, and perform redundant point elimination and curve fitting processing. Finally, generate a set of smooth paths as the output to complete the path planning process.

[0110] 2.2. Pre-allocated Expanded Node Strategy and Environment Perception Mechanism

[0111] The core mechanism of the Rapidly-exploring Random Tree (RRT) algorithm is to perform random sampling in the configuration space to guide the expansion and exploration of the tree. When the RRT algorithm expands a new vertex, it calculates the distance d between this point and the target. If "d" is less than the predetermined threshold "r", it means the algorithm has found the target point and stops the search. If not, it continues the search. Therefore, each time a new node is expanded, the RRT algorithm explores the area centered at this node with a radius of r. The unexplored area of the tree node is called the "unknown area", and the explored area is defined as the "explored area". With each expansion, a new explored area will be created in the unknown area. The RRT algorithm realizes the exploration of the entire space by continuous sampling and expansion. However, this process brings a scenario where some exploration areas are revisited, and some are even visited more than twice. Therefore, the exploration efficiency is measured by the area of the new exploration area explored by the newly expanded vertex and the re-exploration frequency of the known area. When the area of the unexplored area is large, the efficiency is high, but when the area of the known exploration area is large or there are multiple explorations, the efficiency is low.

[0112] Figure 4 (a) illustrates the problem of redundant exploration of the known area. In the figure, the blue area is the explored area, and the overlapping area is the repeated inspection area. The green area is the exploration area of the new vertex, and the new vertex conducts more than three repeated explorations on the area "T" enclosed by the red circle. Figure 4 (b) illustrates the traditional RRT expansion process. This figure shows that vertices x0, x1, and x2 have inspected the "t area" twice. After that, vertex x1 extends towards point x rand and generates a new vertex x new . Unfortunately, this new vertex triggers a third redundant exploration of the "t area". Conducting multiple explorations of the same area is meaningless.

[0113] (1) Pre-allocated Expanded Node Strategy

[0114] In response to the problems mentioned above, the research of the present invention shows that setting the angle between each vertex and its connecting point to 120° can greatly reduce unnecessary space exploration, as Figure 5 shown. When a new point x new extends from vertex x1, the three edges x1 - x0, x1 - x2, and x1 - x newThe included angles between them are all 120°. This reduces the "t area" to Figure 7 30% of (a). The exploration process also avoids repeated exploration more than three times, greatly improving the efficiency.

[0115] The pre-allocated extended node strategy first performs a node traversal on the entire map. All the nodes involved in the traversal work are called array O; then a new vertex x is expanded new : Pre-allocate sim j (where j is the candidate expansion point index, j = 0, 1, 2) to keep the included angles of each vertex at 120°. In addition, if the candidate expansion point set of a vertex is empty, it is classified as a dead vertex and removed from array O. Figure 5 (a) shows that each vertex records its corresponding candidate expansion point x i .sim j (where i is the vertex index in the growth tree T, i = 0, 1,...) If the candidate expansion point set of x i is empty, it is considered a dead vertex and excluded from array O.

[0116] The expansion process of the improved RRT is as shown in Figure 5 (b). After randomly sampling x rand , the nearest vertex x is selected from array O nearest as the starting point. Since x1 is a dead vertex and has been deleted from array O, x2 is selected as the nearest vertex.

[0117] Subsequently, according to the proximity of vertex x new to x rand , vertex x is selected from the candidate expansion points of x2 new . After calculation, sim2 is selected as x new for expansion and x2. sim2 is removed from the candidate set of x2. x2.sim2 coincides with x5.sim2. sim2 is also deleted from the candidate set of x5. Since the candidate set of vertex x5 is now empty, x5 becomes a dead vertex and is removed from array O.

[0118] If the boundary x new -x nearest collides with an obstacle, the algorithm enters the environmental perception stage. Otherwise, x new will be added as a new vertex to tree T, and a set of candidate expansion points will be pre-allocated for x new . It should be noted that the candidate expansion points cannot overlap with any existing points in tree t. This process is as shown in Figure 5 (c).

[0119] (2) Environmental perception mechanism

[0120] Sampling-based algorithms often encounter difficulties when navigating construction scenarios because they lack environmental sensitivity. However, whether a scenario is considered complex depends on the expansion step size. If the step size is much smaller than the width of the feasible path in the scenario, it can be regarded as a wide road; conversely, if the step size is too small, the search accuracy will be too high, resulting in low algorithm efficiency. To solve this problem, the present invention proposes an environmental perception strategy that combines real-time perception of a depth camera, enabling the random tree to quickly identify and pass through the feasible regions in complex scenarios without reducing the step size. During the tree expansion process, when a collision with an obstacle occurs, the expansion is paused and enters the environmental perception stage. When the environment undergoes dynamic changes, real-time perception data is introduced through the depth camera to update the obstacles in the environment, thereby enabling the planning of the robotic arm construction path in dynamic complex scenarios such as construction sites.

[0121] The environmental perception process begins by collecting local spatial information around the x nearest point through local sampling. The local sampling uses the expansion step size as the radius to uniformly sample n points around the vertex to be expanded and stores these points in the point set W. Here, n = 16 is taken as an example. Then, the point set W is divided into two subsets W obs (points in the obstacle region) and W free (points in the free region) according to whether their positions are in the obstacle region. The boundary points between W free and W obs are selected and stored in W bdry , where Figure 6 shows a schematic diagram of the environmental perception mechanism of local sampling, where the red points represent the expanded vertices that have collided, the blue points represent the sampling points W obs , the green points represent the boundary points W bdry , and the yellow and green points represent the W free points. In Figure 6 , the obstacle is identified as a wall obstacle.

[0122] To enable local sampling to accurately identify the passable regions, it is crucial to further clarify the number n of local sampling points and the expansion step size d step . Considering the volume of the robotic arm itself to obtain the width d Width of the minimum feasible channel and the expansion step size d step , to ensure that local sampling can cover the inside of the channel, the spacing between adjacent sampling points cannot exceed the width d Width of the minimum feasible channel. Therefore, the number n of sampling points must satisfy the following formula:

[0123]

[0124] Formula (1) gives the specific formula for partitioning, where π is the angle, d Width is the channel width, d step is the expansion step, and n is the number of sampling points.

[0125] After local sampling, the environment is classified according to the numerical relationship between the point sets W free and W bdry . The environment is divided into two main types: wall obstacles and complex obstacle areas. If W bdry has only two vertices, while W free has more than two vertices, then the environment is classified as a wall obstacle, as Figure 7 shown. In this case, the algorithm exits the environment perception stage and performs resampling. Otherwise, the area is regarded as a complex obstacle area and further subdivided into categories such as entrances, interiors, and exits.

[0126] To cross the obstacle area, x nearest is used as the pivot point, and the expansion step is used as the radius to divide multiple fan-shaped areas. Each fan-shaped area contains only a set of continuous W free points. The resulting n fan-shaped areas are denoted as Z i (i = 1, 2,..., n). The fan-shaped area that does not contain the x parent point is selected as the expansion area. Then a W free point that does not cause a collision is selected from it as the new expansion point x new .

[0127] For example, Figure 7 (a) shows the entrance of the obstacle area. This area is divided into two fan-shaped areas (Z1, Z2), and W bdry2 is selected from the Z2 area as the x new point for expansion because the parent node x parent is located in the Z1 area. The same expansion method is applied to the interior of the obstacle area in Figure 7 (b) and the exit in Figure 7 (c).

[0128] If the number of sampling points n in the fan-shaped area is greater than 2, it is regarded as a multi-channel branch case, as Figure 7 (d) shown. After partitioning, three fan-shaped areas (Z1, Z2, Z3) are obtained. Since Z1 contains the x parent point, two W free points that do not cause a collision are selected from the Z2 and Z3 areas as x new for expansion.

[0129] 3. Two-way Expansion Strategy

[0130] Bidirectional expansion means that by the way that the starting point and the target point attract each other and search towards each other, it naturally overcomes the blindness of the RRT* path exploration, reduces the search time, and ensures the path quality. As Figure 8 shown, essentially, the RRT* algorithm is initialized as two random trees, with the starting point and the target point as the root nodes of each other, that is, the initial parent nodes. The target biasing selection of random points is performed with a certain probability, and the two trees are searched towards each other alternately. When the Euclidean distance dist between the new node generated on any one tree and a certain node on the other tree is less than the threshold threshold, the entire search process ends. After the path search is completed, the connection points on the two random trees are traced back to the starting point and the target point respectively to obtain the planned path.

[0131] 4. Path smoothing mechanism

[0132] The initial path obtained by the CS-RRT* algorithm under the manipulator pose constraint is a continuous line segment connected by nodes in the random tree, with a large number of redundant nodes, which increases the running time of the manipulator. If only the initial path is pruned to remove redundant nodes and the updated path obtained by shortening the path length is not smooth, it will not only affect the stability of the manipulator operation, but also cause unnecessary turning time for the robot during operation. Therefore, it is necessary to perform path optimization.

[0133] As Figure 9 shown, assume that the initial path node sequence P planned by the CS-RRT* algorithm under the manipulator pose constraint is [P1, P2, P3, …, P n , where n is the number of path nodes. First, starting from the starting node P1, connect each node backward in turn and perform collision detection. When a collision occurs, record the current node as P k (1 < k < n), and the nodes between P1 and P k-1 are all redundant nodes. Therefore, the path node sequence P is updated to [P1, P k-1 , P k , …, P n . Then, starting from the node P k-1 , search for collision-free nodes backward in turn. Repeat the above steps to traverse the entire initial path node sequence P, remove all redundant nodes, and obtain the updated path P' with a smaller path cost as [P'1, P'2, P'3, …, P' m , where 0 < m < n.

[0134] Then, perform smoothing optimization on the updated path P' after removing redundant points. To facilitate the description of the path smoothing process, define the path node sequence C to be optimized and the smoothed path sequence Y.

[0135] As Figure 10As shown, the process of implementing node position update essentially needs to meet the following three conditions:

[0136] 1) The node Y after path smoothing q has the smallest deviation from the position of the node C on the path to be optimized q . That is, solve for Y q until ||C q – Y q || has the smallest value, where 0 < q < m.

[0137] 2) The relative offset distance between the node Y after path smoothing q and its neighboring node Y q+1 is the smallest. That is, solve to obtain Y q that satisfies ||Y q – Y q+1 || with the smallest value.

[0138] 3) The connection line between the node Y after path smoothing q and its neighboring node Y q+1 does not collide with obstacles. That is, the smoothed path is as close as possible to the original path.

[0139] To meet the three conditions, the present invention designs a path smoothing method. The specific path smoothing steps are as follows:

[0140] Step1: For conditions 1) and 2), initialize the sequence C to be equal to P'.

[0141] Step2: Let the initial value of the smoothed path sequence Y be C, and use the smoothness objective function shown in Equation (2) to continuously adjust the position of each node Y q in the sequence Y through iteration to obtain the smoothed path, where the starting node and the ending node are not optimized.

[0142]

[0143] Step3: Since the number of path nodes decreases after removing redundant points, resulting in a decrease in the number of nodes on the path to be optimized, it is easy for the smoothed path to deviate from the initial path during the iteration process. As a result, the robotic arm is not only prone to collide with obstacles, but also likely to make the path appear in the unreachable space of the robotic arm, causing condition 3) not to be satisfied. Therefore, as Figure 10 shown, to ensure that condition 3) is met, use the checkLink strategy to perform collision detection on the smoothed path sequence Y obtained in Step2, and let x new = Y q . When the result is true, the algorithm returns the smoothed path sequence Y; when the result is false, it is necessary to select between C q and C in the sequence of nodes on the path to be optimizedq+1 Insert the median value C in between l , and then update the sequence C to [C1, …, C q , C l , C q+1 , …, C 2m-1 , and continue to execute Step2.

[0144] In summary, the present invention proposes a path optimization strategy as Figure 11 shown, which can generate a continuous and smooth path trajectory for the robotic arm, ensuring the stable and fast operation of the robotic arm.

[0145] 5. Robot Arm Collision Detection

[0146] 5.1. Establishment of the Robot Arm Motion Model

[0147] Establish a robot arm motion model to obtain the inverse kinematic equations of each joint of the robot arm. At the same time, establish a geometric model of the robot arm, and use the actual geometric parameters of the robot arm to perform isometric modeling in software to verify the correctness of the pose constraint strategy and limit the working space of the robot arm itself for collision detection between new nodes and the robot arm.

[0148] Taking the UR5e six-axis collaborative robot arm as an example, the present invention uses the improved DH parameter method to establish a kinematic model of the robot arm. The transformation matrix of the joint coordinate system of the robot arm link {i} relative to the link {i-1} is shown in formula (3).

[0149]

[0150] A six-degree-of-freedom robot arm is composed of links connected by six joints. According to the DH parameters and joint limits in Table 1, in the robot coordinate system, when the parameters of each link are known, given the joint radian values θ1, θ2, θ3, θ4, θ5, θ6 of the robot arm, through the homogeneous transformation equation multiplying successively, the forward kinematic relationship of the end of the robot arm relative to the base pose as shown in formula (4) is solved, where attitude = [n o a] (n = [n x , n y , n z ) T , o = [o x , o y , o z ) T , a = [a x , a y , a z ) T ) represents the attitude vector of the end coordinate system of the robot arm; postion = [p x , p y , p z )T Represents the position vector of the end coordinate system of the robotic arm.

[0151] Table 1 Nominal values of DH kinematic parameters of the UR5e robot

[0152]

[0153]

[0154] However, usually, the end pose of the robotic arm is expressed in terms of position parameters and RPY angles. To solve for the joint radian values of the robotic arm when given the end pose parameters of the robotic arm pos = [x, y, z, R, P, Y], the present invention uses Equation (5) to establish the relationship between pos and the end pose vector postion of the robotic arm, thereby obtaining the inverse kinematic equations for each joint of the UR5e robotic arm, as shown in Equation (6).

[0155]

[0156]

[0157] In the formula, A = k1 - a2cosθ2, B = k2 - a2sinθ2.

[0158] 5.2. Conduct collision detection between the robotic arm and obstacles

[0159] To simplify the collision detection problem, as Figure 12 shown, the present invention adopts an envelope method, which can transform the collision detection problem between the robotic arm and obstacles into the collision detection between a cylinder and a cuboid, reducing the computational complexity and improving the efficiency of the robotic arm path planning. After envelope processing, the edges of the obstacles will expand by a certain distance, which can create a safety distance between the robotic arm and the obstacles, further improving the safety of the robotic arm during actual operation, reducing the risk of collisions, and providing an efficient and feasible solution for the robotic arm to plan paths in an environment with irregular obstacles.

[0160] After path planning is completed, inverse kinematics is solved to obtain the joint angles of the robotic arm. For each corresponding position, 8 sets of joint angles can be obtained, and 1 set of joint angles reachable by the robotic arm links is selected. The process of collision detection is as follows: The cylindrical envelope of the robotic arm link (radius r) is simplified to line segment AB, and by expanding the obstacles by r value, the collision detection is transformed into the interference judgment between the line segment and the six planes of the expanded cube.

[0161] First, convert the line segment endpoints to the local coordinate system of the obstacle. Use the parametric equation to solve the potential intersection points of the line segment and each plane, and verify the collision state through the coordinate range. Combine the hierarchical detection strategy (coarse screening with AABB bounding box first, and then fine detection with planes), as shown in Equation (7):

[0162]

[0163] After combining the envelope method and the manipulator kinematic model, let f(x, y, z) be the equation of line segment AB. If the solution set of the equation f(x, y, z) = 0 has an intersection with the above Equation (7), the manipulator collides with the obstacle; if there is no intersection, it means that the manipulator and the obstacle do not collide. The same method is used to solve the judgment of the other links of the manipulator. This algorithm is used for self-position pose obstacle avoidance and obstacle avoidance in space when the manipulator performs operation tasks.

[0164] 5.3. In the masonry task, the generation of the manipulator path not only needs to avoid obstacles, but also needs to consider the order and method of masonry arrangement. Generate the path through the masonry geometric model, so that the path planning can adapt to the actual construction process of the wall (alternate placement of full bricks and half bricks).

[0165] To further improve the actual adaptability of the path planning algorithm in the masonry task, a masonry arrangement model based on the "all - running" masonry method is introduced in the path optimization stage to quantify the spatial interference relationship between the manipulator path and the wall. In the all - running masonry method, the masonry units are arranged in sequence with the long side parallel to the wall surface, and the geometric center points are generated according to the following formula:

[0166] P i,j =(x0 + i·(l + d), y0, z0 + j·(h + d(8)

[0167] where, i represents the serial number of the masonry unit in each layer, j represents the layer number, l is the brick length, h is the brick height, d is the mortar joint width, and (x0, y0, z0) is the starting reference point of the wall. In the path collision detection process, establish the following path risk cost function:

[0168]

[0169] where, the total path risk cost value L total takes values from 0 to 1, and the closer it is to zero, the lower the collision risk; P i,j represents the geometric center point of the i - th masonry unit in the j - th layer, P arm (t) is the position vector at any time on the manipulator path, and σ is the collision sensitivity coefficient. This model models the spatial relationship between the path and the wall masonry, thereby dynamically optimizing the manipulator path and minimizing the potential collision risk between the path and the masonry.

[0170] Normalize the total proxy value L total and define it as the average risk per unit path length:

[0171]

[0172] where T is the path execution duration, i.e., the total time experienced by the robotic arm moving from the starting point to the ending point.

[0173] In this way, the risk range R ∈ (0, ∑ i,j 1], that is, the worst case is that the path coincides with multiple masonry units at every moment. The present invention constructs a path risk cost function based on the spatial distribution of masonry units, further defines the average risk index R per unit path length, and sets the following path safety assessment criteria: R ≥ 0.5: high risk, the path needs to be re-planned; 0.2 ≤ R < 0.5: medium risk, the path exists in the area close to the obstacle and should be optimized; 0.05 ≤ R < 0.2: low risk, executable; R < 0.05: extremely low risk, the path quality is optimal; this risk assessment criterion provides a quantitative basis for the path planning quality evaluation.

[0174] 6. Set up a feedback mechanism

[0175] During the path planning process, introduce a depth camera perception mechanism to achieve real-time mapping and feedback adjustment of the dynamic environment, as Figure 13 shown. The depth camera is used to obtain the dense depth image within the working space of the robotic arm, map it into a set of spatial voxels, and construct the local occupancy map information of the current frame. Based on the change of the point cloud data between consecutive frames, dynamically identify and classify the obstacle state for update.

[0176] Furthermore, adopt a sliding window mechanism to maintain the local map information in real time. When it is detected that there is a newly placed masonry unit or the originally placed masonry unit has moved within the area covered by the original path, update the environmental map M t , and re-evaluate the path feasibility. During the path expansion process, mark the voxels whose occupancy probability exceeds the threshold, and dynamically adjust the expansion node candidate area of the pre-allocated expansion node strategy, so as to ensure the continuity and feasibility of the path based on the updated map, and improve the adaptability of the environmental perception mechanism in the dynamic construction scenario. The environmental map update process can be expressed as:

[0177]

[0178] where: is the occupancy probability inferred from the depth map of the current frame; α is the fusion weight, controlling the fusion degree of the new and old maps; M t+1 is the updated map.

[0179] To achieve real-time perception and mapping of dynamic obstacles, the system combines a depth camera to obtain real-time depth maps and RGB image data in the workspace of the robotic arm. In Figure 13 (a-d), the depth maps (upper layer) and RGB maps (lower layer) collected at different robotic arm postures are respectively shown. The position and geometric changes of obstacles can be clearly perceived through the color gradient changes in the depth maps. Each frame of the depth map is used by the system to update the local map in real time to guide the path planning module to avoid the boundaries of the latest obstacles and achieve dynamic obstacle avoidance and path correction. This mechanism enhances the robustness and real-time performance of the path planning system in complex construction scenarios.

[0180] 7. Simulation and Experiments

[0181] To verify the superiority of the CS-RRT* algorithm in terms of effectiveness and performance, simulation experiments are carried out in MATLAB at the two-dimensional and three-dimensional levels, and the data results of the RRT* algorithm before and after improvement are compared and analyzed. To more comprehensively verify the practicality of the planned path, masonry grasping and placing verification is carried out on the simulation experiment platform to verify the experimental results of the robotic arm movement under the proposed algorithm in the real environment scenario.

[0182] 7.1. Simulation and Analysis in Two-Dimensional Space

[0183] In the two-dimensional simulation experiment, two different scenario models are constructed, namely the simple obstacle scenario and the complex obstacle scenario. The RRT* algorithm, the P-RRT* algorithm (RRT* algorithm with preset nodes), and the CS-RRT* algorithm are compared under the preset two-dimensional map. The size of the scenario is 500mm×500mm. The coordinates of the starting point are [10,10], and the coordinates of the target point are [490,490]. During the path search process, the target bias probability is introduced as 0.3, and the path search step size is set to 5mm. The upper limit of the number of experiment iterations is 5000 times, that is, if a feasible path is found within 5000 times, the task is successful, otherwise the task fails. Due to the randomness involved in path planning, each group of experiments is simulated 100 times, and then the average value is taken to obtain the final result.

[0184] (1) Two-Dimensional Simple Obstacle Experiment

[0185] From Figure 14 (i), it can be clearly observed that in the simple scenario, the RRT* algorithm is a random search of the space, so the randomly sampled points almost cover the entire scenario, resulting in a large search cost, long time, and low efficiency. In contrast, the CS-RRT* algorithm overcomes the randomness of the RRT* algorithm, improves the convergence efficiency, makes the path always update in the search direction towards the target point, thereby reducing the search cost and significantly improving the search speed; From Figure 14(ii) From the search results shown, due to the lack of goal-directedness of P-RRT*, the random trees cannot attract each other, sacrificing the path cost and making the path result not smooth enough; From Figure 14 (iii) From the relationship between the number of iterations and the distance shown, under the characteristic of local asymptotic optimality brought by the self-resetting of the parent node and the re-wiring operation of the RRT* algorithm itself, the CS-RRT* algorithm designed in the present invention incorporates the advantages of the goal-biased strategy and the bidirectional search strategy, and can converge to the local optimal solution with fewer iterations and generate a feasible path faster.

[0186] Table 2 shows the comparison results of the RRT* algorithm and the CS-RRT* algorithm in the experiment:

[0187] Table 2 is the comparison of the experimental results of three RRT* algorithms in a simple two-dimensional scenario

[0188]

[0189] As can be seen from Table 2, in three different scenarios, although paths can be successfully found in the simple scenario, the CS-RRT* algorithm shortens the average path length compared with the traditional RRT* algorithm and greatly improves the path planning search time.

[0190] (2) Two-dimensional complex obstacle experiment

[0191] Figure 15 Shows the comparison chart of the planned paths and the number of iterations of the CS-RRT* algorithm, the RRT* algorithm and the P-RRT* algorithm in a complex two-dimensional scenario, where the line chart represents the number of iterations of the three RRT* algorithms, and the magenta straight line represents the final paths of the three RRT* algorithms.

[0192] According to the experimental results, in a complex two-dimensional scenario, due to its large randomness, RRT* leads to stronger number of iterations and fluctuations, and there is a situation where no path is found after reaching the iteration upper limit. In contrast, thanks to the target area sampling mechanism, the number of iterations of the CS-RRT* algorithm is significantly less than that of the RRT* algorithm, and the number of iterations of multiple experiments remains near the average value, having better stability. The experiment shows that in a complex two-dimensional scenario, the traditional RRT* algorithm has large fluctuations in the number of iterations and a failure rate of 18% due to random sampling, while the CS-RRT* algorithm greatly reduces the average number of iterations after being optimized by integrating multiple mechanisms. The improved algorithm has significant advantages in terms of path length and stability and is more suitable for robots to perform path planning in complex scenarios.

[0193] Figure 15(i) shows the random search process of three RRT* algorithms in a complex scenario. The random sampling points of the RRT* algorithm almost cover the entire scenario, resulting in a large search cost, long time, and low efficiency.

[0194] Table 3 shows the comparison results of three RRT* algorithms in the complex two-dimensional scenario experiment.

[0195] Table 3 Comparison of experimental data of three RRT* algorithms in the complex two-dimensional scenario.

[0196]

[0197] As shown in Table 3, the success rate of finding paths by different algorithms decreases in the complex two-dimensional case. However, the success rate of the CS-RRT* algorithm proposed in the present invention in finding a feasible path within a limited number of iterations is still 100%. Compared with the other two algorithms, the algorithm proposed in the present invention not only shortens the average path length but also significantly reduces the search time required for path planning.

[0198] 7.2 Simulation and Analysis in Three-Dimensional Space

[0199] Since the path planning at the end of the robotic arm is a planning in three-dimensional space, the path search performance of the algorithm in three-dimensional space becomes an important indicator of the effectiveness and superiority of the algorithm proposed in the present invention. The CS-RRT* algorithm is compared with the RRT*, B-RRT* (bidirectional extended RRT* algorithm), and P-RRT* algorithm (RRT* algorithm with preset nodes) in a preset three-dimensional map. The upper limit of the experimental iteration times is set to 10,000, and the target bias threshold P target is set to 0.5. The map range is set to [160, 160, 160], the starting point is set to [10, 10, 10], the target point is set to [150, 150, 150], and the search step size stepsize and distance threshold threshold are set to three groups of [5, 10], [10, 10], and [10, 20]. Considering the randomness of the sampling algorithm, each group of experiments is executed 100 times, and the obstacles are randomly set each time.

[0200] Figure 16 (i) shows the search process of RRT*, B-RRT*, P-RRT*, and CS-RRT* once when the search step size stepsize and distance threshold threshold are set to [10, 10]; the search results are as Figure 16 (ii) shows that each algorithm has obtained a feasible path from the starting point to the target point; Figure 16(ⅲ) shows the relationship between the number of iterations and the distance during the search process for the four algorithms. Since RRT* and P-RRT* are single-tree searches, the distance represents the distance between the newly added node of the random tree and the end point during the search process. While B-RRT* and CS-RRT* are double-tree searches, the distance represents the shortest distance between the two random trees during the search process.

[0201] From Figure 16 (ⅰ) It can be intuitively seen that RRT* is a random search of the space, so the search path fills the entire space, resulting in low search efficiency; P-RRT* overcomes the blindness of the path exploration of the RRT* algorithm and searches in the target direction with a certain probability, accelerating the search speed; the B-RRT* algorithm uses double trees to alternately search towards each other, improving the convergence efficiency to a certain extent. However, from Figure 16 (ⅱ) From the search results shown, due to the lack of target orientation, the random trees cannot attract each other, sacrificing the path cost and making the path result not smooth enough; from Figure 16 (ⅲ) From the relationship between the number of iterations and the distance shown, under the characteristics of the local asymptotic optimality brought by the self-resetting of the parent node and re-wiring operation of the RRT* algorithm itself, the CS-RRT* algorithm designed in the present invention incorporates the advantages of the target bias strategy and the bidirectional search strategy, reducing the number of iterations of the RRT* algorithm from 3589 times to only 85 times required to converge to the local optimal solution, and generating a feasible path faster.

[0202] Three groups of different search step sizes (stepsize) and distance thresholds (threshold) are set, and the average results of 100 experiments in each group are shown in Table 4.

[0203] Table 4 shows the comparison of the search performance of each algorithm in a three-dimensional environment

[0204]

[0205] It can be seen that compared with the RRT* algorithm, the B-RRT* algorithm not only significantly improves the average planning time, with an average increase of about 72.14%, but also has the ability to optimize the path cost; the B-RRT* algorithm sacrifices the path cost as a premise, and the improvement of the average planning time is more obvious, with an average increase of about 80.42%; the CS-RRT* algorithm that incorporates multiple improvement mechanisms still maintains good performance at the level of the average planning time, with an average increase of about 81.22%, and due to the integration and guidance of the environmental perception mechanism and the target bias strategy, it reduces the negative impact on the path cost caused by the bidirectional expansion strategy.

[0206] It can be seen from this that the algorithm proposed in the present invention has great advantages in comprehensively considering the planning time and the path cost, and can quickly and effectively realize the path planning of the manipulator.

[0207] 7.3. Simulation Experiments

[0208] In the robotic arm simulation experiment, the simulation results in MATLAB were loaded into the simulation platform for verification. Figure 17 The robotic arm simulation planning paths under two software are shown. In the left figure, the red cuboid serves as an obstacle, the blue cuboid is set as the simplified link of the robotic arm, and the red dots between the links are the joint points of the robotic arm. In the right figure, the purple trajectory is the planned path of the robotic arm.

[0209] It can be seen from the experiment that in the Cartesian coordinate system, the robotic arm starts from the starting pose on the left side, moves along the pre-planned trajectory, successfully reaches the target pose on the right side, and does not collide with obstacles during the movement process.

[0210] In the experiment, in order to better demonstrate the motion performance of the six-degree-of-freedom robotic arm, the present invention records the paths of each joint during the masonry task. Figure 18 The joint paths of the six joints of the robotic arm are shown. The figure shows the motion trajectories of each joint from the starting position to the target position. These path diagrams intuitively reflect the motion laws of each joint. Through the analysis of these joint paths, the algorithm proposed by the present invention has considerable execution efficiency and accuracy in the robotic arm path planning task.

[0211] Figure 19 The trajectory diagrams of the six joints of the robotic arm are shown, reflecting the motion changes of each joint during the masonry task. Through these trajectory diagrams, the motion paths of each joint from the starting position to the target position can be observed, as well as how to smoothly transition during the path planning process. These diagrams intuitively show the motion laws of the joints, ensure the smoothness of joint motion, reduce unnecessary motions, and verify the performance of the algorithm-planned path in avoiding collisions.

[0212] Figure 20 In a - e, the process of the robotic arm crossing an obstacle to grasp an object is shown; in f - j, the process of the robotic arm re - crossing the obstacle after grasping the object is shown; in k - o, the robotic arm completes the object placement and starts a new round of planning. In the experiment, Figure 19 The step - by - step action sequence of the robotic arm during the grasping and placement tasks is shown. The figure details how the robotic arm performs precise grasping and placement operations in each step, while fully demonstrating the application of the obstacle avoidance mechanism. By gradually analyzing the grasping path and the execution process of the placement action, it can be seen how the improved path planning algorithm adjusts the trajectory of the robotic arm in real time to ensure avoiding collisions with obstacles in a complex environment.

[0213] The present invention proposes an environmental perception robotic arm path planning method algorithm for masonry tasks, aiming to improve the planning efficiency and path quality of the algorithm. Based on this, the depth camera is combined to monitor the masonry environment in real time, enhancing the environmental perception ability of the traditional RRT algorithm. This algorithm adopts a novel node expansion strategy to improve the expansion efficiency of nodes and combines a new environmental perception mechanism to solve the problem of the reachability of the robotic arm in complex construction obstacle areas. MATLAB simulation experiments show that this method significantly speeds up the generation of the search path and the convergence of the algorithm. The smoothed path reduces the motion cost of the robotic arm and improves the path quality. Simulation experiments show that the improved algorithm performs excellently in an environment with dense obstacles and dynamic changes, and can effectively avoid collisions and generate smooth and executable trajectories.

[0214] The following is the device embodiment of the present invention, which can be used to execute the method embodiment of the present invention. For details not disclosed in the device embodiment, please refer to the method embodiment of the present invention.

[0215] The present invention also provides an environmental perception robotic arm path planning system for masonry tasks, which runs the steps of an environmental perception robotic arm path planning method for masonry tasks. The system includes:

[0216] A model establishment module, which is used to establish an environmental perception robotic arm path planning model. The model is based on the RRT* algorithm using a two-way expansion strategy, and uses a pre-allocated expansion node strategy and an environmental perception mechanism to update the new nodes of the RRT* algorithm;

[0217] A path planning module, which is used to input the obtained robotic arm kinematic model, environmental data, starting point and target point of the robotic arm movement into the environmental perception robotic arm path planning model to obtain an initial path;

[0218] A path optimization module, which is used to perform operations such as removing redundant points and path smoothing on the initial path based on the robotic arm pose constraint to obtain the robotic arm movement path for masonry tasks;

[0219] A path update module, which is used for the robotic arm to perform masonry tasks using the obtained robotic arm movement path. During the masonry task, the environmental data is updated through the depth camera perception mechanism, and then the robotic arm movement path for masonry tasks is updated.

[0220] In another embodiment of the present invention, a terminal device is further provided. The terminal device includes a processor and a memory. The memory is used to store a computer program, and the computer program includes program instructions. The processor is used to execute the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, and is suitable for implementing one or more instructions. Specifically, it is suitable for loading and executing one or more instructions to implement the corresponding method flow or corresponding function. The processor described in the embodiment of the present invention can implement the operation of a method for path planning of an environment-aware robotic arm for masonry tasks.

[0221] In another embodiment of the present invention, a storage medium is further provided, specifically a computer-readable storage medium (Memory). The computer-readable storage medium is a memory device in the terminal device and is used to store programs and data. It can be understood that the computer-readable storage medium here can include both the built-in storage medium in the terminal device and, of course, the extended storage medium supported by the terminal device. The computer-readable storage medium provides a storage space that stores the operating system of the terminal. And in this storage space, one or more instructions suitable for being loaded and executed by the processor are also stored. These instructions can be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory or a non-volatile memory, such as at least one disk memory. One or more instructions stored in the computer-readable storage medium can be loaded and executed by the processor to implement the corresponding steps of the method for path planning of an environment-aware robotic arm for masonry tasks in the above embodiments.

[0222] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) that contain computer-usable program code.

[0223] The present application is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to the embodiments of the present application. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate a device for realizing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0224] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing devices to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including an instruction device that realizes the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0225] These computer program instructions can also be loaded onto a computer or other programmable data processing devices, such that a series of operation steps are executed on the computer or other programmable devices to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable devices provide steps for realizing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0226] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that: still modifications or equivalent replacements can be made to the specific embodiments of the present invention, and any modifications or equivalent replacements that do not depart from the spirit and scope of the present invention shall be covered by the protection scope of the claims of the present invention.

Claims

1. An environmental perception robotic arm path planning method for masonry tasks, characterized in that, The specific steps are as follows: S1. Establish an environmental perception robotic arm path planning model. The model is based on the RRT* algorithm using a bidirectional expansion strategy, and adopts a pre-allocated expansion node strategy and an environmental perception mechanism to update the new nodes of the RRT* algorithm; S2. Input the obtained robotic arm kinematic model, environmental data, starting point and target point of the robotic arm movement into the environmental perception robotic arm path planning model to obtain an initial path; S3. Based on the robotic arm pose constraints, perform operations to remove redundant points and smooth the path on the initial path to obtain a robotic arm movement path for the masonry task; S4. The robotic arm uses the obtained robotic arm movement path to perform the masonry task. During the masonry task, the environmental data is updated through the depth camera perception mechanism. Repeat S1-S3 to update the robotic arm movement path for the masonry task.

2. The path planning method of an environment perception manipulator for masonry tasks according to claim 1, characterized in that In S1, the specific steps of the pre-allocated expansion node strategy are as follows: 1) Candidate expansion point setting The RRT* algorithm randomly samples x rand and then selects the nearest vertex x nearest as the starting point. For vertex x nearest pre-allocate candidate expansion points sim j , and set the angle between each vertex and its connecting point to 120°; 2) Select a new vertex According to the proximity of vertex x new to x rand , select vertex x from the candidate expansion points of x nearest ; new ; 3) Perform collision detection If the boundary x new -x nearest does not collide with the obstacle, then x new is added as a new vertex to the random tree, and a set of candidate expansion points is pre-allocated for x new The candidate expansion points cannot overlap with any existing points in the tree; If the boundary x new -x nearest collides with an obstacle, use the environmental perception mechanism to process the obstacle area; When the candidate expansion point set of the vertex is empty, it is classified as a dead vertex and removed.

3. The path planning method of an environment perception manipulator for masonry tasks according to claim 2, characterized in that, In S1, the specific steps of the environmental perception mechanism are as follows: 1) Local sampling Using the extended step size as the radius, uniformly sample n points around x nearest and divide them into an obstacle area point set, a free area point set, and a boundary point set between the obstacle area and the free area. The boundary point set belongs to the free area point set; The number of sampling points n must satisfy the following formula: where π is the angle, and d Width is the channel width, and the minimum feasible channel width is obtained from the volume of the robotic arm itself, d step is the expansion step size, and n is the number of sampling points; 2) Environmental classification: When there are only two vertices in the obstacle area point set and more than two vertices in the free area point set, the environment is a wall obstacle and random sampling is performed again; otherwise, the environment is a complex obstacle area; When the environment is a complex obstacle area, taking node x nearest as the fulcrum and the expansion step length as the radius, divide multiple fan-shaped areas, and each fan-shaped area contains only one set of continuous W free points; Taking the fan-shaped area that does not contain the parent node x parent as the expansion area. In the case of a single-channel situation, select a W free point that does not cause a collision from the expansion area as the new expansion point x new ; in the case of a multi-channel branch situation, select multiple W free points that do not cause a collision from the expansion area as the new expansion point x new .

4. A path planning method for an environment perception manipulator for masonry tasks according to claim 1, characterized in that In S3, the steps to optimize the initial path are as follows: Preliminary smoothing process: Based on the robotic arm pose constraints, perform operations to remove redundant points and smooth the path on the initial path to obtain a smooth path; Robotic arm collision detection path smoothing process: Based on the smooth path, judge the reachability of the robotic arm link pose through the principle of robotic arm inverse kinematics, and use the AABB bounding box model to judge whether the robotic arm collides with obstacles to obtain a smooth path after collision detection; Masonry task path smoothing process: Obtain the geometric center point of the i-th block and j-th layer of masonry in the masonry task, and use the distance between the geometric center point and the position vector at any time in the robotic arm path to further perform collision detection between the robotic arm and obstacles, and obtain an optimal path where the robotic arm pose is reachable and there is no collision in the masonry task as the robotic arm movement path.

5. A path planning method for an environment perception robotic arm for masonry tasks according to claim 4, characterized in that, In the masonry task path smoothing process, a path risk cost function is established through the distance between the geometric center point and the position vector at any time in the robotic arm path. The path risk cost function is as follows: Among them, the total path risk proxy value L total takes values from 0 to 1, and the closer it is to zero, the lower the collision risk; P i,j represents the geometric center point of the i-th block and the j-th layer of masonry, which is determined according to the masonry method; P arm (t) is the position vector at any moment in the robotic arm path, and σ is the collision sensitivity coefficient; Normalize the total path risk value L total as the average risk per unit path length, defined as: where T is the path execution duration, i.e., the total time experienced by the robotic arm from the starting point to the ending point; R ∈ (0, ∑ i,j 1]; Path safety evaluation criteria: R≥0.5: High risk, the path needs to be re-planned; 0.2≤R<0.5: Medium risk, the path exists in the area close to the obstacle, and should be optimized; 0.05≤R<0.2: Low risk, executable; R<0.05: Extremely low risk, the path quality is optimal.

6. The path planning method of an environment perception manipulator for masonry tasks according to claim 1, characterized in that In S4, the depth camera perception mechanism is specifically: Use the depth camera to obtain a dense depth image in the robotic arm workspace, map it into a spatial voxel set, and construct the local occupancy map information of the current frame; The sliding window mechanism is adopted to maintain the local occupancy map information in real time. When it is detected that there is a newly placed masonry unit or a movement of the originally placed masonry unit within the area covered by the original path, the environmental map is updated, and the path feasibility is re-evaluated. During the path expansion process, the voxels with occupancy probability exceeding the threshold are marked, and the corresponding expansion node candidate area in the pre-allocated expansion node strategy is dynamically adjusted. The environmental map update process can be expressed as: Where: M t is the environmental map; is the occupancy probability inferred from the current frame depth map; α is the fusion weight; M t+1 is the updated map.

7. An environmental perception robotic arm path planning system for masonry tasks, characterized in that, Including: A model establishment module, which is used to establish an environmental perception robotic arm path planning model. The model is based on the RRT* algorithm using a two-way expansion strategy, and updates the new nodes of the RRT* algorithm by using the pre-allocated expansion node strategy and the environmental perception mechanism. A path planning module, which is used to input the obtained robotic arm kinematic model, environmental data, starting point and target point of the robotic arm movement into the environmental perception robotic arm path planning model to obtain an initial path. A path optimization module, which is used to perform redundant point removal and path smoothing operations on the initial path based on the robotic arm pose constraints to obtain a robotic arm movement path for the masonry task. A path update module, which is used for the robotic arm to perform the masonry task using the obtained robotic arm movement path. During the masonry task, the environmental data is updated through the depth camera perception mechanism, and then the robotic arm movement path for the masonry task is updated.

8. A terminal 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 computer program, it implements the steps of a method for environmental perception robotic arm path planning for a masonry task as described in any one of claims 1 to 6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of a method for environmental perception robotic arm path planning for a masonry task as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of a method for environmental perception robotic arm path planning for a masonry task as described in any one of claims 1 to 6.

Images