A potential field method for multi-machine cooperative obstacle avoidance path planning in grasping.

Abstract

This invention relates to the field of multi-machine collaborative grasping and obstacle avoidance planning technology, specifically a potential field method for multi-machine collaborative grasping and obstacle avoidance path planning. The method involves acquiring and superimposing grids, obstacle directions, and target directions to form directional relationships. It then classifies state levels based on position offset and contact force, filters paths based on directional consistency, and eliminates limiting points to form continuous paths. It adjusts for path intersections between multiple machines, and screens for conflicts based on spatial relationships, replacing approaching path segments to form a collaborative grasping and obstacle avoidance path. This invention reduces path offset by superimposing obstacles and target directions to form constraints; it links path selection and operation by combining position offset and contact force to classify states, reducing grasping deviation; it enhances continuity by filtering paths and eliminating limiting points through direction matching; it forms collaboration by identifying and avoiding path intersections; and it improves operational stability by screening path-obstacle relationships and replacing approaching segments.

Description

Technical Field

[0001] This invention relates to the field of multi-machine collaborative grasping and obstacle avoidance planning technology, and in particular to a multi-machine collaborative grasping and obstacle avoidance path planning method based on potential field method. Background Technology

[0002] The multi-robot collaborative grasping and obstacle avoidance planning technology field encompasses mobile robot path planning, spatial position modeling, dynamic constraint processing, multi-agent task allocation, collision detection, and obstacle avoidance control. The core of this technology lies in the unified modeling and constraint processing of the motion trajectories of multiple actuators within the same workspace, enabling them to complete the target object grasping task while satisfying position, velocity, and acceleration constraints. The overall technical system includes discrete representation of the environment space, geometric modeling of obstacles, definition of actuator structural parameters, description of target object pose, establishment of constraints on the relative positional relationships between multiple actuators, setting of trajectory generation rules, and the process of outputting motion control commands.

[0003] Among them, the multi-machine cooperative grasping and obstacle avoidance path planning method based on potential field refers to a method for constructing the motion path of multiple actuators based on artificial potential field functions. Specifically, it involves setting an attraction function for the target object's position, a repulsion function for the obstacle boundary, and an interaction function for the distance between multiple actuators to form a superimposed potential field distribution. This method encompasses environmental rasterization processing, obstacle boundary point coordinate extraction, target object 3D coordinate input, actuator initial pose parameter input, attraction function expression setting, repulsion function expression setting, distance constraint function setting between multiple actuators, potential field superposition calculation rules, and a processing method for generating discrete path point sequences based on the potential field gradient direction.

[0004] Traditional methods drive path evolution based on potential value changes, lacking directional consistency constraints. Under complex obstacle distributions, path swaying or redundant detours are prone to occur. Path generation is based solely on spatial position changes, without incorporating force and offset information at the execution end. During the grasping phase, there is a mismatch between the path and the operation state, which can easily lead to unstable contact. Inter-machine communication mainly relies on distance constraints and lacks a mechanism for recognizing path intersections, which can easily lead to conflicts or waiting in intersection areas. After the path results are generated, there is a lack of point-by-point verification process for obstacle boundaries. Some path points are not close enough to the boundary, which may trigger temporary avoidance or even interrupt the task during the execution phase. Summary of the Invention

[0005] To address the technical problems existing in the prior art, this invention provides a multi-machine cooperative obstacle avoidance path planning method based on the potential field method. The technical solution is as follows: A multi-machine cooperative obstacle avoidance path planning method based on potential field method includes the following steps: S1: Obtain the set of environmental grid coordinates, the set of obstacle boundary point coordinates, the center coordinates of the target object, and the current position coordinates of multiple drones in the multi-drone collaborative grasping operation area. Extract the direction of the obstacle boundary point and map it to the grid position. Extract the direction of the target object center pointing to the grid and superimpose them to obtain the potential field direction distribution set. S2: Obtain the coordinates of the actuator at the gripper end of each UAV and the coordinates of the target object gripping reference point to form the offset state; obtain the contact force data to identify continuous sampling differences to form the disturbance state; divide the offset state and disturbance state into level intervals to obtain the gripping state classification sequence. S3: Extract the direction information of path candidate points based on the potential field direction distribution set, read the level information in combination with the grabbing state hierarchical sequence, identify the direction consistency and retain the consistent direction candidate points, remove the restricted interval path points, retain the allowed interval path points, connect the path points to form the path direction, and obtain the multi-machine path candidate sequence. S4: Call the multi-drone path candidate sequence to extract the path point direction information of each UAV, compare the path direction to identify the intersecting path segments and perform avoidance adjustment, connect the paths to form a continuous path relationship, and obtain the multi-drone obstacle avoidance path sequence; S5: The multi-machine obstacle avoidance path sequence is called to extract the spatial position relationship of the path points, and conflicting path points are identified and removed by comparing the path points with the obstacle boundary. The consistency between the path direction and the target object position is identified, and the path segment is replaced to form a grasping proximity path relationship, so as to obtain the multi-machine collaborative grasping obstacle avoidance path result.

[0006] As a further embodiment of the present invention, the potential field direction distribution set includes direction field intensity value, direction continuity index, and spatial distribution density; the grasping state classification sequence includes stability level identifier, error tolerance range, and force feedback consistency index; the multi-machine path candidate sequence includes path node identifier set, path connectivity index, and path feasibility weight; the multi-machine obstacle avoidance path sequence includes avoidance trajectory set, path conflict priority, and path cooperation matching relationship; and the multi-machine cooperative grasping obstacle avoidance path result includes target approach path set, path safety assessment index, and path optimization parameters.

[0007] As a further aspect of the present invention, the step of obtaining the potential field direction distribution set is as follows: S101: Obtain the set of environmental grid coordinates, the set of obstacle boundary point coordinates, and the current position coordinates of multiple UAVs. For each obstacle boundary point, compare the coordinate components with the coordinates of the adjacent grid, extract the direction component symbols and determine the direction quadrant, write the direction quadrant correspondence into the grid index position, and perform weighted superposition processing on multiple direction components in the same grid to obtain the boundary direction index table. S102: Obtain the center coordinates of the target object, compare the coordinate components of the center coordinates of the target object with the coordinates of each grid in the set of environmental grid coordinates, extract the direction component pointing to the target and perform normalization processing, fill the direction component into the corresponding grid index position and form a direction distribution structure, uniformly standardize the direction state of each grid, and obtain the target pointing distribution map. S103: Based on the boundary direction index table and the target pointing distribution map, perform superposition processing on the direction components of the same grid index position, perform normalization processing on the superposition result and update the grid direction index correspondence, and perform consistency correction processing on all grid directions to obtain the potential field direction distribution set.

[0008] As a further aspect of the present invention, the step of obtaining the hierarchical sequence of the grasping state is as follows: S201: Obtain the coordinates of the actuator at the gripper end of each UAV and the coordinates of the target object gripping reference point. Perform spatial position comparison processing between the actuator coordinates and the gripping reference point coordinates. Identify the spatial deviation direction and extract the deviation level based on the coordinate component direction relationship. Write the corresponding deviation level of each UAV into the corresponding index position and form an offset relationship expression structure to obtain the offset level distribution map. S202: Collect contact force data at the gripper end, perform numerical comparison processing on adjacent sampling points of continuous sampling data, extract the force value change trend and identify the fluctuation range, map the fluctuation amplitude of each sampling moment to the corresponding sampling index position and form a perturbation expression structure, perform consistency correction processing on the time series perturbation change, and obtain the perturbation amplitude change curve; S203: Based on the offset level distribution map and the disturbance amplitude change curve, perform corresponding relationship matching processing on the same time index positions, perform interval division operation according to the offset level and disturbance amplitude distribution, perform level mapping on the state within the interval and write it into the sequence index position, and complete the level update processing on the time index to obtain the capture state level sequence.

[0009] As a further aspect of the present invention, the step of obtaining the multi-machine path candidate sequence is as follows: S301: Obtain the potential field direction distribution set, extract the spatial position and corresponding direction component information of the path candidate points, write the direction components of the path candidate points into the index position to form a direction expression structure, perform normalization processing on the direction components and unify the representation form to obtain the candidate direction expression map. S302: Based on the candidate direction representation map and the grabbing state hierarchical sequence, read the level information corresponding to the path candidate points, perform direction component and level identifier matching processing, identify the direction consistency relationship and filter and retain the path candidate points with consistent directions, write the filtering results into the index position, and obtain the direction consistency candidate set. S303: Based on the direction-consistent candidate set, read the path candidate point level interval identifier, perform path point interval division processing, remove restricted interval path points and retain allowed interval path points, perform adjacency relationship connection processing on the retained path points to form a continuous path direction, and obtain a multi-machine path candidate sequence.

[0010] As a further aspect of the present invention, the step of obtaining the multi-machine obstacle avoidance path sequence is as follows: S401: Call the multi-drone path candidate sequence, extract the path point coordinates and direction component information of each UAV, write the path point direction component into the path index position and form a direction expression structure, perform unified representation processing on the path point direction and associate it with the UAV identifier to obtain the path direction distribution map; S402: Based on the path direction distribution map, perform path direction comparison processing, identify the direction intersection area between different UAV paths and extract the position of the intersection path segment, write the intersection path segment index into the corresponding path identifier and associate it with the direction component information to obtain the path intersection segment set; S403: Based on the path intersection segment set, perform path point proximity relationship identification processing, adjust the directional components of the approaching path segments and update the path index position, connect the adjusted path segments with the original path direction to form a continuous path direction, complete the consistency connection processing of all paths, and obtain the multi-machine obstacle avoidance path sequence.

[0011] As a further aspect of the present invention, the step of obtaining the multi-machine collaborative grasping obstacle avoidance path result is as follows: S501: Call the multi-machine obstacle avoidance path sequence, extract the spatial coordinate information of path points and the connection relationship of path segments, compare the spatial position of path point coordinates with the spatial position of obstacle boundary, identify the overlapping area of ​​path points and obstacle boundary and extract the index position of conflicting path points, write the conflicting path points into the path identifier set to obtain the path conflict point set. S502: Based on the set of conflict points, perform conflict path point removal processing, perform adjacency connection processing on the removed path points according to the original path sequence relationship, perform continuous connection processing on broken path segments to form updated path directions, write the updated path to the path index position, and obtain a path continuity relationship graph. S503: Based on the path continuity graph, extract the path direction information and compare it with the spatial position of the target object to identify the consistent relationship between the path segment and the target object position. Replace inconsistent path segments and reconstruct the path connection relationship. Perform unified connection processing on all path segments to obtain the multi-machine collaborative grasping and obstacle avoidance path result.

[0012] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following: In this invention, by mapping the obstacle boundary direction and the target direction together onto the grid space and forming a directional superposition relationship, the path generation is constrained by directional consistency, reducing disordered offsets; by combining the execution end position offset and contact force difference to classify state levels, the path selection and operation state are linked, reducing the risk of offset during the grasping process; by retaining candidate points and eliminating path points in restricted intervals through direction matching, the path connection is made more coherent; by cross-identifying the multi-machine path direction and performing approach avoidance, the multi-machine movement forms a cooperative relationship; by screening the spatial relationship between path points and obstacles and replacing the approaching target segment, the path takes into account both obstacle avoidance and approach process, improving the overall operational stability. Attached Figure Description

[0013] Figure 1 This is a flowchart of the method of the present invention; Figure 2 This is a flowchart illustrating the process of obtaining the potential field direction distribution set in this invention. Figure 3 This is a flowchart illustrating the process of obtaining the hierarchical sequence of the capture state in this invention. Figure 4 This is a flowchart illustrating the process of obtaining multi-machine path candidate sequences according to the present invention. Figure 5 This is a flowchart illustrating the process of obtaining the multi-machine obstacle avoidance path sequence according to the present invention. Figure 6 This is a flowchart illustrating the process of obtaining the obstacle avoidance path results for multi-machine collaborative grasping in this invention. Detailed Implementation

[0014] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0015] refer to Figures 1 to 6 A multi-machine cooperative obstacle avoidance path planning method based on potential field method includes the following steps: S1: Obtain the set of environmental grid coordinates, the set of obstacle boundary point coordinates, the center coordinates of the target object, and the current position coordinates of multiple drones in the multi-drone collaborative grasping operation area. Extract the direction information of the obstacle boundary points and map it to the corresponding grid position. Extract the direction information of the target object center coordinates pointing to the grid coordinates. Superimpose the two types of directions to form the grid direction pointing relationship and obtain the potential field direction distribution set. S2: Obtain the coordinates of the actuator at the gripper end of each UAV and the coordinates of the target object gripping reference point, and compare the positions to form an offset state. Obtain the contact force data at the gripper end, identify continuous sampling differences to form a disturbance state, complete the division of the offset state and disturbance state to form a level interval, and obtain the gripping state level sequence. S3: Extract the direction information of path candidate points based on the potential field direction distribution set, read the corresponding level information by combining the capture state hierarchical sequence, identify the direction consistency and retain the consistent direction candidate points, remove the restricted interval path points, retain the allowed interval path points, connect the path points to form a continuous path direction, and obtain the multi-machine path candidate sequence. S4: Call the multi-drone path candidate sequence to extract the path point direction information of each drone, compare the path direction to identify the intersecting path segments, perform avoidance adjustment of the approaching path segment, connect the path directions to form a continuous path relationship, and obtain the multi-drone obstacle avoidance path sequence; S5: Call the multi-machine obstacle avoidance path sequence to extract the spatial position relationship of path points, identify conflicting path points by comparing the path points with the obstacle boundary space, remove conflicting path points and connect paths, identify the consistency between the path direction and the target object position, and replace path segments to form a grasping proximity path relationship, and obtain the multi-machine collaborative grasping obstacle avoidance path result.

[0016] The potential field orientation distribution set includes orientation field intensity values, orientation continuity indicators, and spatial distribution density. The grasping state classification sequence includes stability level identifiers, error tolerance ranges, and force feedback consistency indicators. The multi-machine path candidate sequence includes a path node identifier set, path connectivity indicators, and path feasibility weights. The multi-machine obstacle avoidance path sequence includes an avoidance trajectory set, path conflict priority, and path cooperation matching relationships. The multi-machine collaborative grasping and obstacle avoidance path results include a target approach path set, path safety assessment indicators, and path optimization parameters. Please see Figure 2 The steps to obtain the potential field direction distribution set are as follows: S101: Obtain the set of environmental grid coordinates, the set of obstacle boundary point coordinates, and the current position coordinates of multiple UAVs. For each obstacle boundary point, compare the coordinate components with the coordinates of the adjacent grid, extract the direction component symbols and determine the direction quadrant, write the direction quadrant correspondence into the grid index position, and perform weighted superposition processing on multiple direction components in the same grid to obtain the boundary direction index table. In acquiring the potential field direction distribution set, the system first performs refined environmental modeling of the operating area, dividing the 50m x 50m monitoring space into a digital grid with a resolution of 0.5m, constructing a spatial index matrix containing 10,000 independent nodes. A multi-sensor fusion positioning system retrieves the 3D coordinate set of obstacle boundary points in the environment in real time and obtains the absolute position coordinates of multiple participating drones. During execution, for each obstacle boundary point coordinate, the system automatically searches for all neighboring grid centers within a 1.5m radius and compares their coordinate components one by one. By calculating the numerical difference between the grid center and the boundary point along the horizontal and vertical axes, the sign of the direction component is extracted. If the difference is positive, it is mapped to a positive component; if negative, it is mapped to a negative component, thus determining the direction quadrant of the grid relative to the obstacle. After writing the determined direction quadrant correspondence into the grid index position, for cases where the same grid is affected by multiple boundary points, the system performs weighted superposition processing, with the weight coefficients allocated according to the inverse square of the distance. For example, a higher weight is assigned to a grid cell 0.7 meters from the boundary point to reflect a strong repulsive force, while a lower weight is assigned to a grid cell 1.2 meters from the boundary point. The system performs vector summation on all participating weighted components and re-determines the quadrant distribution based on the synthesized vector, thereby eliminating the interference of single boundary noise on the potential field direction and obtaining a boundary direction index table.

[0017] S102: Obtain the center coordinates of the target object, compare the coordinate components of the center coordinates of the target object with the coordinates of each grid in the set of environmental grid coordinates, extract the direction component pointing to the target and perform normalization processing, fill the direction component into the corresponding grid index position and form a direction distribution structure, uniformly standardize the direction state of each grid, and obtain the target pointing distribution map. The system obtains the geometric center coordinates of the target object, for example, locking it at (45.0, 45.0) in the coordinate system. For each valid grid in the environmental grid coordinate set, the system extracts its center coordinates one by one and compares them in detail with the center coordinates of the target object. By performing subtraction, the system can extract the direction component pointing to the target from each grid, which represents the gravitational tendency guiding the UAV toward the target. To ensure that the magnitude of the guiding force across the entire field is comparable and does not overflow with increasing distance, the system performs strict normalization on the extracted direction components pointing to the target, standardizing the magnitude of all direction vectors to a standard unit value. The system fills these normalized direction components into the corresponding grid index positions, forming the initial direction distribution structure. During processing, the system also performs uniform standardization on the direction state of each grid. If the grid position overlaps with a known obstacle entity area, its direction state is invalidated; if it is in free passage space, its gravitational direction is maintained. This process completes the comprehensive coverage of the gravitational potential field within the operating space, resulting in a target pointing distribution map.

[0018] S103: Based on the boundary direction index table and the target pointing distribution map, perform superposition processing on the direction components of the same grid index position, perform normalization processing on the superposition result and update the grid direction index correspondence, perform consistency correction processing on all grid directions, and obtain the potential field direction distribution set.

[0019] Based on the boundary direction index table and the target pointing distribution map, the system performs complex superposition processing on the direction components at the same grid index position. This superposition is essentially a vector synthesis of boundary repulsion and target attraction. During the synthesis process, if a grid is subjected to both the repulsion of the obstacle and the attraction of the target, the system will determine the resultant force direction of the grid based on the vector sum of the two components. To maintain the rigor of the potential field definition, the system performs normalization processing on the superimposed synthesis result again and updates the correspondence of the grid direction indices in real time accordingly. After generating the initial set, the system performs consistency correction processing on all grid directions to address the potential field's ...

[0020] Please see Figure 3 The steps for obtaining the hierarchical sequence of capture states are as follows: S201: Obtain the coordinates of the actuator at the gripper end of each UAV and the coordinates of the target object gripping reference point. Perform spatial position comparison processing between the actuator coordinates and the gripping reference point coordinates. Identify the spatial deviation direction and extract the deviation level based on the coordinate component direction relationship. Write the corresponding deviation level of each UAV into the corresponding index position and form an offset relationship expression structure to obtain the offset level distribution map. The system acquires the real-time spatial coordinates of the gripper actuators of each participating drone and extracts the coordinates of multiple pre-set gripping reference points on the target object. Taking the first drone as an example, the system performs precise physical spatial position comparison between its actuator coordinates and the corresponding gripping reference point coordinates. By performing subtraction operations on the X, Y, and Z coordinate components, the system can accurately identify the spatial deviation direction of the actuator relative to the reference point and calculate the geometric magnitude of the deviation vector as the deviation magnitude. For example, when the measured deviation magnitude is 0.57 meters, this value represents the degree of deviation between the current robotic arm end effector and the ideal gripping position. The system writes the deviation magnitude corresponding to each drone to the corresponding index position in real time and constructs a dynamic offset relationship expression structure. This structure not only records the offset of a single drone but also forms an offset magnitude distribution map that can intuitively reflect the spatial distribution accuracy of the collaborative system by aggregating data from multiple drones.

[0021] S202: Collect contact force data at the gripper end, perform numerical comparison processing on adjacent sampling points of continuous sampling data, extract the force value change trend and identify the fluctuation range, map the fluctuation amplitude of each sampling moment to the corresponding sampling index position and form a perturbation expression structure, perform consistency correction processing on the time series perturbation change, and obtain the perturbation amplitude change curve; During the instant of contact between the drone and the target object and throughout the continuous grasping process, the system utilizes a force sensor integrated into the gripper to collect contact force data with high precision at a frequency of 100 Hz. The collected continuous sampling sequence is input into the processing module to perform comparison processing of values ​​from adjacent sampling points to extract the instantaneous trend of force value changes. The system focuses on identifying the fluctuation range of force value within a specific sliding time window, such as calculating the standard deviation of force value within 0.1 seconds. If the standard deviation reaches 0.85 Newtons, the system will determine it as a state with significant disturbance. The system accurately maps the fluctuation amplitude identified at each sampling moment to the corresponding time sampling index position, thereby forming a disturbance representation structure. To eliminate spurious numerical fluctuations caused by sensor electronic noise or mechanical vibration, the system performs consistency correction processing on the time series disturbance changes, using a combination of median filtering and moving average to extract the true mechanical feedback characteristics, resulting in a disturbance amplitude change curve.

[0022] S203: Based on the offset level distribution map and the disturbance amplitude change curve, perform corresponding relationship matching processing on the same time index position, perform interval division operation according to the offset level and disturbance amplitude distribution, perform level mapping on the state within the interval and write it into the sequence index position, and complete the level update processing of the time index to obtain the capture state level sequence.

[0023] Based on the offset magnitude distribution map and the disturbance amplitude variation curve, the system performs correspondence matching processing on the two heterogeneous data at the same time index position. The system performs interval division based on the joint distribution of offset magnitude and disturbance amplitude, and evaluates the current grasping state through a preset logical judgment model. During the evaluation process, the system introduces a weighted scoring mechanism, assigning weight coefficients of 0.6 and 0.4 to the offset magnitude and disturbance amplitude, respectively. For example, when the normalized offset value is 0.37 and the normalized disturbance value is 0.25, the comprehensive score calculated by the system will be used for level mapping. The system divides the state within the interval into three levels: stable, metastable, and risky, and writes the corresponding level identifier into the sequence index position. As the task progresses, the system continuously updates the level of the time index, transforming the original physical quantity into a level signal reflecting the safety of the operation, resulting in a grasping state classification sequence.

[0024] Please see Figure 4 The steps for obtaining the multi-machine path candidate sequence are as follows: S301: Obtain the potential field direction distribution set, extract the spatial position and corresponding direction component information of the path candidate points, write the direction components of the path candidate points into the index position to form a direction expression structure, perform normalization processing on the direction components and unify the representation form to obtain the candidate direction expression map. The system invokes a set of potential field direction distributions and extracts the spatial coordinates of candidate path points and their corresponding potential field direction components based on the current task planning requirements. During extraction, the system precisely writes the direction component of each candidate point into its corresponding index position, constructing a preliminary direction representation structure. To ensure that potential forces of different locations and intensities have equal guiding effect during path generation, the system performs normalization on all direction components. By dividing each component of the direction vector by the total magnitude of the vector, the system achieves complete uniformity in the direction representation form. This process eliminates computational offsets caused by numerical differences, ensuring that all path points possess clear and standardized guiding attributes, resulting in a candidate direction representation map.

[0025] S302: Based on the candidate direction representation map and the capture state hierarchical sequence, read the corresponding level information of the path candidate points, perform direction component and level identifier matching processing, identify the direction consistency relationship and filter and retain the path candidate points with consistent directions, write the filtering results into the index position, and obtain the direction consistency candidate set. By combining the candidate direction representation map and the grasping state hierarchy sequence, the system synchronously reads the hierarchy information of candidate path points at corresponding times. During execution, the system rigorously matches the direction components of candidate points with their respective hierarchy identifiers. To select the most advantageous path points for task advancement, the system performs direction consistency relationship identification, evaluating the degree of consistency by calculating the cosine of the angle between the candidate point's direction vector and the target's pointing vector. The system sets a consistency threshold of 0.86; a candidate path point is retained only when the angle between the candidate point's direction and the target direction is less than 30 degrees, and the corresponding grasping state hierarchy sequence is at a stable or metastable level. For candidate points with excessive pointing deviations or grasping states assessed as risky, the system performs filtering and elimination. The system writes all verified candidate points to an index position, obtaining a direction-consistent candidate set.

[0026] S303: Based on the direction-consistent candidate set, read the level interval identifier of the path candidate point, perform path point interval division processing, remove path points in restricted intervals and retain path points in allowed intervals, perform adjacency connection processing on the retained path points to form a continuous path direction, and obtain a multi-machine path candidate sequence.

[0027] Based on the candidate set with consistent direction, the system reads the level interval identifiers of each path candidate point and initiates path point interval partitioning processing. In this step, the system further refines the passage priority of path points, logically isolates points in different level intervals, and completely removes any path points that overlap with restricted intervals or potential conflict areas. The system performs adjacency relationship connection processing on the retained high-quality path points, searching for node sequences that can be connected into lines based on the Euclidean distance and time continuity characteristics between points. To ensure the physical feasibility of the path, the system removes connection points with excessively large spans and uses a graph search algorithm to construct the optimal connection path in the retained point set. This process transforms discrete candidate points into logically continuous motion trajectories, resulting in a multi-machine path candidate sequence.

[0028] Please see Figure 5 The steps for obtaining the multi-machine obstacle avoidance path sequence are as follows: S401: Call the multi-drone path candidate sequence, extract the path point coordinates and direction component information of each UAV, write the path point direction component into the path index position and form a direction expression structure, perform unified representation processing on the path point direction and associate it with the UAV identifier to obtain the path direction distribution map. The system retrieves a multi-drone path candidate sequence and extracts the coordinates of all path points for each UAV within the planning period, along with their associated directional components. To achieve collaborative scheduling among multiple UAVs, the system writes these path point directional components one by one into the path index location and constructs a multi-dimensional directional representation structure, which includes the UAV identifier, timestamp, spatial coordinates, and normalized directional vector. The system performs unified representation processing on the path point directions, converting the vector information into an easily computed angle form and ensuring that this data is closely associated with the UAV's unique identifier. In this way, the system establishes a dynamic path direction distribution map in the global coordinate system, resulting in the path direction distribution map.

[0029] S402: Based on the path direction distribution map, perform path direction comparison processing, identify the direction intersection area between different UAV paths and extract the position of the intersection path segment, write the intersection path segment index into the corresponding path identifier and associate it with the direction component information to obtain the path intersection segment set; Based on the path direction distribution map, the system performs complex path alignment comparison processing. The system traverses the pathpoint sequences of all drones, calculating the spatial distance between different drone paths within the same time slice. When a preset safety envelope is identified where the pathpoint distance between any two drones is less than 2 meters, the system determines it as a directional intersection area and extracts the specific location indexes of these intersecting path segments. The system not only records the spatial coordinates of the conflict but also writes the intersection path segment indexes into the corresponding path identifiers and associates them with the directional component information of each drone within that area. By comparing the directional vector angles of the intersection segments, the system can identify whether it is a head-on conflict, a convergence in the same direction, or a lateral intrusion, thus obtaining a set of path intersection segments.

[0030] S403: Based on the path intersection segment set, perform path point proximity relationship identification processing, adjust the directional components of the approaching path segments and update the path index position, connect the adjusted path segments with the original path direction to form a continuous path direction, complete the consistency connection processing of all paths, and obtain the multi-machine obstacle avoidance path sequence.

[0031] Based on the set of path intersection segments, the system performs path point proximity relationship identification processing to eliminate potential inter-aircraft collision hazards. For identified intersection path segments, the system adjusts the directional components of the approaching segments according to the relative motion vectors of the two aircraft. For example, if the two aircraft are flying head-on, the system introduces a lateral diversion guiding force, deflecting their respective path directional components 15 degrees to the side. The system updates the values ​​in the path index positions in real time and connects the adjusted path segments with the original non-conflicting path segments. To avoid abrupt motion changes caused by path adjustment, the system uses an interpolation algorithm to optimize the continuity at the connection points, ensuring a smooth transition of the path direction. After consistent connection processing of all path segments, a multi-aircraft obstacle avoidance path sequence is obtained.

[0032] Please see Figure 6 The steps for obtaining the obstacle avoidance path results in multi-machine collaborative grasping are as follows: S501: Call the multi-machine obstacle avoidance path sequence, extract the spatial coordinate information of path points and the connection relationship of path segments, compare the spatial position of path point coordinates with the spatial position of obstacle boundary, identify the overlapping area of ​​path points and obstacle boundary and extract the index position of conflicting path points, write the conflicting path points into the path identifier set, and obtain the path conflict point set. The system invokes a multi-machine obstacle avoidance path sequence, extracting the precise spatial coordinates of each path point and the logical connections between path segments. During execution, the system performs a 3D spatial comparison of these path point coordinates with the spatial positions of obstacle boundaries in the environment. The system calculates the minimum vertical distance from each path point to the nearest obstacle's geometric edge. Once a path point is identified as being within a safety redundancy threshold of 0.3 meters from an obstacle boundary, it is determined to be an overlapping area. The system quickly extracts the index positions of these conflicting path points and writes them into a path identifier set. This process performs a secondary verification of path safety, resulting in a set of path conflict points.

[0033] S502: Based on the set of conflict points, perform conflict point removal processing, perform adjacency connection processing on the removed path points according to the original path sequence, perform continuous connection processing on broken path segments to form updated path directions, write the updated path to the path index position, and obtain the path continuity graph. Based on the set of conflicting path points, the system performs a process to remove conflicting path points, eliminating all points that threaten flight safety from the original sequence. For broken path segments appearing after the removal of points, the system performs adjacency connection processing according to the original path's continuity relationship, and introduces a local path reconstruction algorithm to perform continuity connection processing on the broken parts. The system uses the previous safe point as the starting point and the next safe point as the target point, and under the guidance of the local potential field, re-searches and generates a series of updated path points that avoid obstacles. For example, a path point that originally collided with an obstacle is shifted outward by 0.4 meters. The system rewrites these corrected and reconstructed updated paths back into the path index positions, obtaining a path continuity graph.

[0034] S503: Based on the path continuity graph, extract the path direction information and compare it with the spatial position of the target object to identify the consistent relationship between the path segment and the target object position. Replace inconsistent path segments and reconstruct the path connection relationship. Perform unified connection processing on all path segments to obtain the multi-machine collaborative grasping and obstacle avoidance path results.

[0035] Based on the path continuity graph, the system extracts the direction information of each path segment and compares it with the spatial position of the target object for directional consistency. This step aims to ensure that the path can still effectively guide the target after multiple obstacle avoidance adjustments. The system identifies the directional consistency between path segments and the target object's position. If a path segment is found to have a directional deviation angle greater than 60 degrees due to obstacle avoidance, the system will perform a replacement process. The system will reconstruct the connection relationship of this segment, introducing a stronger gravitational bias to correct its direction. The system performs uniform connection and smoothing processing on all path segments, eliminating any unnatural inflection points, and obtaining the multi-machine collaborative grasping obstacle avoidance path result.

[0036] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A multi-machine cooperative obstacle avoidance path planning method based on potential field method, characterized in that, Includes the following steps: S1: Obtain the set of environmental grid coordinates, the set of obstacle boundary point coordinates, the center coordinates of the target object, and the current position coordinates of multiple drones in the multi-drone collaborative grasping operation area. Extract the direction of the obstacle boundary point and map it to the grid position. Extract the direction of the target object center pointing to the grid and superimpose them to obtain the potential field direction distribution set. S2: Obtain the coordinates of the actuator at the gripper end of each UAV and the coordinates of the target object gripping reference point to form the offset state; obtain the contact force data to identify continuous sampling differences to form the disturbance state; divide the offset state and disturbance state into level intervals to obtain the gripping state classification sequence. S3: Extract the direction information of path candidate points based on the potential field direction distribution set, read the level information in combination with the grabbing state hierarchical sequence, identify the direction consistency and retain the consistent direction candidate points, remove the restricted interval path points, retain the allowed interval path points, connect the path points to form the path direction, and obtain the multi-machine path candidate sequence. S4: The multi-drone path candidate sequence is called to extract the path point direction information of each UAV, the path direction is compared to identify the intersecting path segments and the avoidance adjustment is performed, the paths are connected to form a continuous path relationship, and the multi-drone obstacle avoidance path sequence is obtained.

2. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 1, characterized in that: The potential field direction distribution set includes direction field intensity value, direction continuity index, and spatial distribution density; the grasping state classification sequence includes stability level identifier, error tolerance range, and force feedback consistency index; the multi-machine path candidate sequence includes path node identifier set, path connectivity index, and path feasibility weight; and the multi-machine obstacle avoidance path sequence includes avoidance trajectory set, path conflict priority, and path cooperative matching relationship.

3. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 1, characterized in that: The steps for obtaining the potential field direction distribution set are as follows: S101: Obtain the set of environmental grid coordinates, the set of obstacle boundary point coordinates, and the current position coordinates of multiple UAVs. For each obstacle boundary point, compare the coordinate components with the coordinates of the adjacent grid, extract the direction component symbols and determine the direction quadrant, write the direction quadrant correspondence into the grid index position, and perform weighted superposition processing on multiple direction components in the same grid to obtain the boundary direction index table. S102: Obtain the center coordinates of the target object, compare the coordinate components of the center coordinates of the target object with the coordinates of each grid in the set of environmental grid coordinates, extract the direction component pointing to the target and perform normalization processing, fill the direction component into the corresponding grid index position and form a direction distribution structure, uniformly standardize the direction state of each grid, and obtain the target pointing distribution map. S103: Based on the boundary direction index table and the target pointing distribution map, perform superposition processing on the direction components of the same grid index position, perform normalization processing on the superposition result and update the grid direction index correspondence, and perform consistency correction processing on all grid directions to obtain the potential field direction distribution set.

4. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 1, characterized in that: The steps for obtaining the hierarchical sequence of the capture state are as follows: S201: Obtain the coordinates of the actuator at the gripper end of each UAV and the coordinates of the target object gripping reference point. Perform spatial position comparison processing between the actuator coordinates and the gripping reference point coordinates. Identify the spatial deviation direction and extract the deviation level based on the coordinate component direction relationship. Write the corresponding deviation level of each UAV into the corresponding index position and form an offset relationship expression structure to obtain the offset level distribution map. S202: Collect contact force data at the gripper end, perform numerical comparison processing on adjacent sampling points of continuous sampling data, extract the force value change trend and identify the fluctuation range, map the fluctuation amplitude of each sampling moment to the corresponding sampling index position and form a perturbation expression structure, perform consistency correction processing on the time series perturbation change, and obtain the perturbation amplitude change curve; S203: Based on the offset level distribution map and the disturbance amplitude change curve, perform corresponding relationship matching processing on the same time index positions, perform interval division operation according to the offset level and disturbance amplitude distribution, perform level mapping on the state within the interval and write it into the sequence index position, and complete the level update processing on the time index to obtain the capture state level sequence.

5. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 1, characterized in that: The steps for obtaining the multi-machine path candidate sequence are as follows: S301: Obtain the potential field direction distribution set, extract the spatial position and corresponding direction component information of the path candidate points, write the direction components of the path candidate points into the index position to form a direction expression structure, perform normalization processing on the direction components and unify the representation form to obtain the candidate direction expression map. S302: Based on the candidate direction representation map and the grabbing state hierarchical sequence, read the level information corresponding to the path candidate points, perform direction component and level identifier matching processing, identify the direction consistency relationship and filter and retain the path candidate points with consistent directions, write the filtering results into the index position, and obtain the direction consistency candidate set. S303: Based on the direction-consistent candidate set, read the path candidate point level interval identifier, perform path point interval division processing, remove restricted interval path points and retain allowed interval path points, perform adjacency relationship connection processing on the retained path points to form a continuous path direction, and obtain a multi-machine path candidate sequence.

6. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 1, characterized in that: The steps for obtaining the multi-machine obstacle avoidance path sequence are as follows: S401: Call the multi-drone path candidate sequence, extract the path point coordinates and direction component information of each UAV, write the path point direction component into the path index position and form a direction expression structure, perform unified representation processing on the path point direction and associate it with the UAV identifier to obtain the path direction distribution map. S402: Based on the path direction distribution map, perform path direction comparison processing, identify the direction intersection area between different UAV paths and extract the position of the intersection path segment, write the intersection path segment index into the corresponding path identifier and associate it with the direction component information to obtain the path intersection segment set; S403: Based on the path intersection segment set, perform path point proximity relationship identification processing, adjust the directional components of the approaching path segments and update the path index position, connect the adjusted path segments with the original path direction to form a continuous path direction, complete the consistency connection processing of all paths, and obtain the multi-machine obstacle avoidance path sequence.

7. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 1, characterized in that, The method further includes step S5: S5: The multi-machine obstacle avoidance path sequence is called to extract the spatial position relationship of the path points, and conflicting path points are identified and removed by comparing the path points with the obstacle boundary. The consistency between the path direction and the target object position is identified, and the path segment is replaced to form a grasping proximity path relationship, so as to obtain the multi-machine collaborative grasping obstacle avoidance path result. The results of the multi-machine collaborative grasping and obstacle avoidance path include the target approach path set, path safety assessment index, and path optimization parameters.

8. The multi-machine cooperative grasping and obstacle avoidance path planning method based on the potential field method according to claim 7, characterized in that: The steps for obtaining the multi-machine collaborative grasping obstacle avoidance path results are as follows: S501: Call the multi-machine obstacle avoidance path sequence, extract the spatial coordinate information of path points and the connection relationship of path segments, compare the spatial position of path point coordinates with the spatial position of obstacle boundary, identify the overlapping area of ​​path points and obstacle boundary and extract the index position of conflicting path points, write the conflicting path points into the path identifier set to obtain the path conflict point set. S502: Based on the set of conflict points, perform conflict path point removal processing, perform adjacency connection processing on the removed path points according to the original path sequence relationship, perform continuous connection processing on broken path segments to form updated path directions, write the updated path to the path index position, and obtain a path continuity relationship graph. S503: Based on the path continuity graph, extract the path direction information and compare it with the spatial position of the target object to identify the consistent relationship between the path segment and the target object position. Replace inconsistent path segments and reconstruct the path connection relationship. Perform unified connection processing on all path segments to obtain the multi-machine collaborative grasping and obstacle avoidance path result.

Images