A dual-arm robot obstacle avoidance path planning method based on an inertial probability road map algorithm

Abstract

The application relates to a dual-arm robot obstacle avoidance path planning method based on an inertial probability road map algorithm, which comprises the following steps: based on a dual-arm kinematic model, discretizing a working space into adaptive resolution grids and carrying out structured sampling, constructing a connection relationship of the grid structure based on inertial connectivity judgment, and generating a connected graph; constructing a differentiated evaluation function to search for a dual-arm candidate path combination in the connected graph, calculating a conflict risk, and screening out a path combination with the optimal cost and no inter-arm conflict as a pre-feasible path; carrying out independent collision detection, locking a successful path if the single-arm path detection is successful or the single-arm path detection fails, updating a local connected graph, searching for a local pre-feasible path, and generating a collision-free feasible path; and adopting a seven-degree polynomial interpolation method to carry out global interpolation fitting, obtaining a continuous pose trajectory in a Cartesian space, and converting the continuous pose trajectory into robot control instructions. Compared with the prior art, the application has the advantages of considering planning efficiency, collaboration and obstacle avoidance reliability, etc.

Description

Technical Field

[0001] This invention relates to the field of path planning for dual-arm robots, and in particular to a method for obstacle avoidance path planning for dual-arm manipulators based on an inertial probabilistic roadmap algorithm. Background Technology

[0002] Currently, intelligent manufacturing and special-purpose equipment are rapidly upgrading towards multi-arm collaboration, autonomous obstacle avoidance, and high-precision execution. Dual-arm robots, with their strong collaborative capabilities and high operational flexibility, are widely used in complex and confined space operations. Traditional manual inspection and operation modes suffer from low efficiency, high safety risks, and poor environmental adaptability, making it difficult to meet the demands for high precision, high repeatability, and high safety. Promoting the development of robot systems towards autonomy, intelligence, and collaboration has become an industry consensus. However, dual-arm robots face challenges in actual operation, such as strong motion coupling, overlapping workspaces, and complex obstacle distribution. Issues such as inter-arm interference, path conflicts, and insufficient planning efficiency are prominent, becoming key bottlenecks restricting their engineering applications.

[0003] Existing obstacle avoidance path planning methods for robotic arms are mostly designed for single-arm scenarios. Directly transferring them to dual-arm systems often leads to insufficient coordination, planning deadlock, and high computational costs. Graph search and traditional sampling methods involve significant repetitive computations when building the environment model, and collision detection involves numerous invalid calculations. Some dual-arm planning methods merely separate the two robotic arms into independent single-arm planning steps, failing to achieve global coordination and conflict prediction, which can easily lead to path redundancy and frequent replanning in confined spaces. While traditional lazy probabilistic roadmap algorithms can reduce initial computational costs, they still have significant shortcomings in areas such as differentiated dual-arm operations, local path repair, and safe inter-arm coordination, failing to balance planning efficiency, obstacle avoidance reliability, and dual-arm coordination. Therefore, there is an urgent need to propose improved path planning methods adapted to dual-arm operations. The paper "Obstacle Avoidance Path Planning for a Dual-Arm Inspection Robot Based on an Improved Lazy-PRM Algorithm" discloses an obstacle avoidance path planning method for a dual-arm robot. This method includes setting priorities for path search and obstacle avoidance strategies for each arm to prevent path conflicts between the two arms; constructing a kinematic model of the two arms and using a grid method to model the arms and workspace; setting posture preferences for different arms to assist in selecting inverse kinematics solutions; designing pseudo-random sampling points to construct a connected graph and evaluation functions for different arms to obtain the collision-free path with the highest evaluation value. This method maintains no path conflicts in a dual-arm operating environment, effectively achieving collision-free shortest path planning. The effectiveness of the strategy is verified from the perspectives of path length, computation time, and trajectory. However, in the collision detection method, if only a single arm's path detection fails, a complete re-search of the path is still required, resulting in high computational costs. Geometric collision detection also suffers from a large amount of invalid computation.

[0004] In summary, there is currently a lack of a method for obstacle avoidance path planning for dual-arm robotic robots based on lazy probabilistic roadmap algorithms to solve or partially solve the above problems. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the existing technology by providing a dual-arm robot obstacle avoidance path planning method based on an inertial probabilistic path map algorithm, so as to solve or partially solve the problems of high computational cost of complete path planning, large amount of invalid calculation in collision detection of traditional geometric calculation, path redundancy, and inter-arm conflicts.

[0006] The objective of this invention can be achieved through the following technical solutions: This invention provides a method for obstacle avoidance path planning of a dual-arm robot based on a lazy probabilistic path graph algorithm, specifically including: Based on the constructed kinematic model of the dual robotic arms, the work space is discretized into an adaptive resolution grid and structured sampling is performed. The connection relationship of the grid structure is constructed based on the inertial connectivity determination. An initial global probability path map is constructed based on the set of sampling points and the connection relationship, and a connected graph is generated. Construct differentiated evaluation functions for the main arm and auxiliary arm, search for candidate path combinations of the two arms in the connected graph based on the differentiated evaluation functions, calculate the conflict risk of the candidate path combinations of the two arms, and select the path combination with the best cost and no inter-arm conflict as the pre-feasible path based on the calculation results; Independent collision detection is performed based on the pre-feasible path. If both arm paths pass the detection, the pre-feasible path is taken as a collision-free feasible path. If neither arm path passes the detection, the connected graph is updated and a new candidate path combination for both arms is searched based on the differential evaluation function. If one arm path is successfully detected while the other fails, the successfully detected robotic arm path is locked, the spatial trajectory of the successfully detected robotic arm path is projected into the grid and marked as a temporary obstacle space, the local connected graph is updated, and a local pre-feasible path is searched in the local connected graph to generate a collision-free feasible path. The collision-free feasible path is subjected to global interpolation fitting using the seventh-order polynomial interpolation method to obtain the continuous pose trajectory in Cartesian space and convert it into a robot control command package to realize obstacle avoidance path planning for the dual-arm robot.

[0007] As a preferred technical solution, the independent collision detection includes: First, the jump path trajectory is planned using the robot joint space interpolation method; then, based on the trajectory of the robotic arm end effector, the links and joints of the robot body are projected into a three-dimensional grid space model using a forward kinematics model; finally, based on the grid positions of different objects, collisions are determined by grid overlap.

[0008] As a preferred technical solution, the quantization formula for determining whether a collision has occurred based on grid overlap is expressed as follows: in, Indicates the collision determination status. The set of grid cells corresponding to the space occupied by the main body. C represents the set of grid cells occupied by fixed obstacles; when C=1, it is determined that a collision has occurred on the corresponding robotic arm path; when C=0, it is determined that there is no collision on the path.

[0009] As a preferred technical solution, the steps corresponding to the failure of both arm paths to pass the detection include: Set the connectivity matrix of the collision path to a disconnected state. The overall update formula for the connectivity matrix is: in, It is a connected Boolean matrix. For raster coordinates, The set of node pairs corresponding to the collision path is defined; the connectivity graph is updated by updating the connectivity matrix, and the pre-feasible paths of the two arms are searched again according to the differentiated evaluation function until the independent collision detection is passed.

[0010] As a preferred technical solution, the step of updating the locally connected graph includes: Define a temporary obstacle grid set to store the grids occupied by the successful robot arm trajectory, locate the node pairs corresponding to the collision path segments of the failed robot arm, set the connectivity matrix of the node pairs to a disconnected state, and perform a local update on the connected graph, using the following formula: in, It is a connected Boolean matrix. This is to locate the node pairs corresponding to the collision path segments of the failed robotic arm.

[0011] As a preferred technical solution, the steps corresponding to successful single-arm path detection and failed single-arm path detection further include: Based on the locally updated connected graph, taking the previous valid node of the failed robot arm collision segment as the new starting point and the original target pose as the ending point, a new locally pre-feasible path is searched in the locally connected graph. Collision detection is then performed again on the new locally pre-feasible path, and the determination formula is as follows: in, This indicates the collision determination situation involving temporary obstacles. The set of grid cells corresponding to the space occupied by the main body. A set of grid cells occupied by fixed obstacle spaces. To define a temporary obstacle grid set; when =0, perform dual-arm collaborative path collision detection between the locked successful path and the corrected failed arm path, when =1, repeat the local update of the connectivity matrix and local path search operation only for the failed node pairs until the merging verification is successful.

[0012] As a preferred technical solution, the steps for calculating the conflict risk include: For each candidate path combination, a real-time conflict predictor is run, and the two candidate paths are discretized into spatiotemporal nodes with the same time step. The spatial Euclidean distance between the ends of the two arms at the time step is calculated, and the conflict probability at the corresponding time is obtained by combining the preset safety distance threshold. The average conflict risk value of the entire candidate path is then calculated to determine the conflict risk.

[0013] As a preferred technical solution, the cost-optimized screening step includes: Set a conflict risk threshold. If the average conflict risk value is less than the conflict risk threshold, keep the initial weight coefficients and calculate the path performance cost of the two robotic arms respectively. If the average conflict risk value is greater than the conflict risk threshold, an online adaptive weight adjustment mechanism is triggered. The main arm weight coefficient remains unchanged, and only the auxiliary arm weight coefficient is dynamically updated. Then, the auxiliary arm path performance cost is calculated based on the adjusted weight coefficient, the comprehensive dual-arm path performance cost is obtained, and the path combination with the optimal cost is selected.

[0014] As a preferred technical solution, the step of converting the continuous pose trajectory in Cartesian space into a continuous angle trajectory in joint space through an inverse kinematics model, discretizing the continuous angle trajectory in joint space, encapsulating the discretized continuous angle trajectory in joint space into a standardized control instruction package according to the robot control system communication protocol, and sending it to each joint servo driver according to a preset time step.

[0015] As a preferred technical solution, the global interpolation fitting step includes: The path point generation based on the collision-free feasible path takes into account position, velocity, acceleration, and jerk, resulting in a continuous and smooth motion trajectory. A seventh-order polynomial interpolation function is used with time as the independent variable. By substituting the position, velocity, acceleration, and jerk constraint polynomials of the starting point, intermediate key transition points, and ending point of the collision-free feasible path at a specified time, the coefficients of the polynomials are solved to obtain the interpolation function expression, thereby achieving a high-order smooth fit of the collision-free feasible path.

[0016] Compared with the prior art, the present invention has at least one of the following beneficial effects: (1) This invention constructs a full-scene independent collision detection and dual-arm path classification and local update mechanism for pre-feasible paths. It avoids robot motion singularities by interpolating joint space and then judges the collision results by grid overlap quantization. When a single-arm path fails, there is no need for overall replanning. Only the successful path is locked and marked as a temporary obstacle. The local update and local replanning of the connected graph are completed for the failed path segment. Only when both arms fail is the connected graph updated and searched again. This solves the problem of high computational cost of replanning all paths and achieves the technical effect of reducing computational redundancy and improving the fault tolerance and practicality of path planning.

[0017] (2) The independent collision detection method constructed in this invention performs low-order grid query pre-filtering in the inertial probability roadmap stage to reduce invalid calculations. In the precise collision detection stage, the robotic arm links and joints are projected onto the grid through positive kinematics to perform quantitative calculation of the overlap of the grid occupancy state. This solves the problem of a large number of invalid calculations in collision detection of traditional geometric calculation, realizes intuitive and efficient detection of collision judgment, and provides a reliable basis for subsequent classification processing.

[0018] (3) This invention constructs a collaborative obstacle avoidance path planning system based on dynamic conflict prediction, full-scene collision quantification detection, and local and overall differentiated replanning. It deeply integrates the inertial probability road map algorithm, designs a differentiated evaluation function in the path search stage, adds end effector motion direction guidance and configures independent weight coefficients, so that the main arm prioritizes the core operation requirement of the shortest path, and the auxiliary arm dynamically adjusts the weight according to the conflict risk and actively makes way for the main arm to avoid obstacles, thereby achieving collaborative obstacle avoidance. It solves the problems of motion coupling of the two robotic arms, path redundancy, and inter-arm conflict, and achieves the technical effect of taking into account planning efficiency, collaboration and obstacle avoidance reliability. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of the method flow of the present invention; Figure 2 This is a schematic diagram of the dual-arm path collision detection process of the present invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0021] To address the problems existing in the prior art, this embodiment provides a method for obstacle avoidance path planning for a dual-arm robot based on a lazy probabilistic path graph algorithm, such as... Figure 1 As shown, it specifically includes: S1. Kinematic Modeling of Dual Robotic Arms and 3D Rasterized Environment Modeling. This step aims to construct a kinematic relationship model of the dual robotic arms and transform the workspace into a computable grid structure, providing a basic model support for subsequent path planning. The specific operations are as follows: S1.1. Establish a three-dimensional rectangular coordinate system with the center of the dual-arm base as the global origin O; define the position vectors from the center of the two arm bases to the origin O as follows: and ,in For the base of the two robotic arms in A fixed spacing along the axis, determined based on the actual robot hardware parameters; the real-time pose of the dual robotic arms in joint space is set as follows. ,in These represent the angle values ​​of the six joints of robotic arm #1. The joint numbers are 1-6, from the base to the end effector. These represent the angle values ​​of the six joints of robotic arm #2.

[0022] S1.2. An improved DH method is used to construct the kinematic model of the robotic arm. A DH coordinate system is established for each joint, and the homogeneous transformation matrix between adjacent joints is defined. This is used to describe the position and attitude transfer relationship between joints. The formula code for the matrix is ​​as follows: in, Joint angle, For the link torsion angle, The length of the link. This represents the linkage offset.

[0023] S1.3. The inverse kinematics solution of the robot can be obtained by inversely deriving the pose formula and using the given Cartesian space pose information to obtain the corresponding joint angles of each robotic arm. Since the inverse kinematics solution is not unique, a specific pose often corresponds to multiple sets of joint angle values. Therefore, the choice of the inverse solution has a significant impact on the path planning results, especially when the robotic arm itself is at risk of collision. This embodiment mainly considers the elbow joint, specifically the influence of the parameter settings of joints 2-3 on the inverse kinematics solution, based on the robotic arm's function. The robotic arm's workspace is usually directly in front. The rotation angle of the elbow joint affects the arm's height depending on different values. By imposing restrictions on the elbow joint rotation angles of different robotic arms, the motion postures of the two robotic arms can be differentiated, achieving dynamic obstacle avoidance between the robotic arms. The pose formula is expressed as: S1.4. By reversing the Cartesian spatial pose formula above, we can obtain the joint angles, i.e., the inverse kinematic solution. However, the inverse solution is not unique. To avoid motion conflicts between the two robotic arms, based on the robotic arm function settings, we focus on constraining the elbow joint, i.e., the rotation angle of joints 2-3. The constraint formula is as follows: in, For the number of joints, This indicates that it applies to robotic arm 1. This indicates that it applies to robotic arm 2.

[0024] S1.5. Determine the 3D spatial range to be modeled based on the maximum boundary of the work scene, and construct an adaptive resolution grid based on this range. The grid size can be dynamically adjusted according to the complexity of the environment and the requirements of planning precision. Grid cells are encoded into different state values ​​to clearly represent environmental attributes: areas without any objects occupying them and that are safe to pass through are defined as passable spaces. In the known environmental structure scanning stage, all 3D projected grids occupied by fixed obstacles determined by sensors or drawings are collectively referred to as fixed obstacle spaces. In the collision detection and real-time path evaluation stage of path planning, the spatial volume occupied by the robot body is converted into equivalent grid data based on the inverse kinematics calculation and pose solution of all moving links of the current robot, which is called the body-occupied space. Or, obstacle grids dynamically marked during forward prediction are called temporary confined spaces. Among them, for temporary confined spaces, the modeling of this invention also performs forward-looking dynamic identification and marking updates, which usually maps the potential movement path and transient area of ​​another robotic arm in collaborative operation within a certain time window. These pre-labeled and real-time updated raster maps form a three-dimensional environmental situation map that can intuitively reflect the structure of the real environment and accurately depict free paths and dynamic danger zones. This provides a fast and reusable spatial topology data foundation for subsequent rasterized improvement of inertial probabilistic road map sampling and global obstacle avoidance path planning based on it.

[0025] S2. Construct a gridded improved inertial probabilistic roadmap sampling and connectivity graph.

[0026] S2.1. To improve the coverage of sampling points in the effective workspace and enhance the efficiency of connected graph construction, one of the core improvements of this invention is the proposal of a structured sampling strategy based on refined environmental representation. This method no longer performs completely random sampling across the entire space, but instead relies on a 3D raster map with clear spatial attributes constructed in S1.5 for guidance. The key to this method is that the algorithm uses a traversable spatial raster based on dynamic resolution, i.e., an area with an S-state value of 0, as the basic index unit for sampling, generating a core representative location point within each raster unit.

[0027] The generation of core representative location points begins by determining the total number of grid cells within the complete workspace defined in S1.5 using the following formula. : in, , These represent the upper and lower bounds of the robot's operating space. The grid length is... For the spatial dimension of robot operation, For the first For each grid cell identified as a passable space, using its spatial index and the unique identifier of the current work scene as seeds, a deterministic pseudo-random coordinate point with a small, controllable offset near the center of the grid is generated through an iterative method. The formula is as follows: in, for A dimensional random vector, where each component takes values ​​from 1 to 2. This ensures the macroscopic uniformity of the final distribution of all sampling points and the reproducibility of the algorithm.

[0028] This method fundamentally differs from traditional random point scattering, enabling it to efficiently and comprehensively cover all safe, potential path passage regions. This provides a highly representative initial candidate node set for subsequent connectivity assessment and path search, effectively improving the convergence speed of subsequent graph search algorithms.

[0029] S2.2. After obtaining the structured set of sampling points, lazy connectivity determination is introduced as another core feature of the algorithm. During the initial connected graph construction phase, the algorithm does not immediately perform detailed geometric collision detection between all node pairs, but instead performs a fast collision prediction step.

[0030] Specifically, the algorithm performs pairwise analysis on all sampled points. When determining whether a connection between two points constitutes a valid path, it prioritizes checking a series of 3D grid cells traversed by the path. By efficiently querying the state identifier S(x,y,z) of the underlying grid map, it can instantly determine whether there are known static obstacles (S=1) or predicted dynamic obstacles (S=2) on the path. Only when all grid cells along the entire path are passable spaces are the two-point pair considered potentially connected, and a preliminary connection marker is established in the connectivity matrix. This mechanism significantly reduces the computational cost of constructing the initial connectivity graph because a large number of sampled point connections that inevitably intersect with obstacle areas are filtered out early on, whether they are fixed environmental obstacles or the expected movement area of ​​another robotic arm.

[0031] S2.3. Using the set of sampling points generated in S2.1 as nodes of the graph, and based on the potential connectivity relationships formed in S2.2, a sparse but well-defined initial global probabilistic path graph is constructed. The connected graph constructed at this stage is a highly forward-looking and reusable structure. It not only includes spatial connectivity relationships based on the known static structure of the environment, but more importantly, by dynamically projecting the space occupied by the current dual robotic arms at each moment and synchronously updating it on the grid map, i.e., marking it as space occupied by the arms or temporarily restricted space, the algorithm can implicitly consider the potential motion conflict possibilities of the dual robotic arms that evolve over time in this connected graph. This significantly reduces the iterative and costly environmental exploration and computation time required for global resampling and graph structure reconstruction in each dual-arm collaborative path planning task, making the entire path planning framework particularly suitable for dual-arm inspection operation scenarios that require multiple consecutive task planning.

[0032] S3. Use the dual-arm differential evaluation function to search for paths and match the pre-feasible path with no inter-arm conflict and the optimal path cost.

[0033] S3.1. Obtain the inspection task requirements of the dual-arm robot and determine the starting pose of the first robotic arm. and target pose and the initial pose of robotic arm No. 2. and target pose The points corresponding to these four poses are added as additional sampling points to the connected graph constructed by S2, and their connectivity with the original sampling points is determined, and the connected Boolean matrix is ​​updated.

[0034] S3.2. Design a distributed evaluation criterion that balances system task priority and real-time risk assessment to simultaneously evaluate the applicability of two independent paths for robotic arms. Based on the heuristic evaluation of the traditional A* algorithm, this criterion introduces a target attraction dynamic adjustment module for the slave arm, optimizing the motion orientation while ensuring the end effector approaches the target.

[0035] The evaluation function for the main boom, i.e., the core working boom, is: in, From the starting point to the current node The actual cost, For the current node Heuristic cost estimation to the target point These are the weighting coefficients. This indicates that the cost between two points is calculated using Euclidean distance. The path performance evaluation of the main arm always takes the shortest total travel and the highest process efficiency as the sole objective, so the weight of the main arm is fixed at 1.

[0036] The initial evaluation function for the auxiliary arm, i.e., the cooperating arm, is: in, This represents the Manhattan distance between two points. The initial evaluation goal of the arm is to adjust the direction weighting coefficient. It guides the attitude of its end effector, prioritizing obstacle avoidance coordination to allow for adjustments in case of possible yielding behavior.

[0037] Based on this differentiated evaluation function, multiple combinations of candidate paths for both arms are searched in the connected graph. The real-time conflict risk assessment in this step is only applicable to scenarios where valid candidate paths are found for both arms. A real-time conflict predictor is run for each candidate path, discretizing the two candidate paths into spatiotemporal nodes with the same time step, and calculating the... The spatial Euclidean distance at the ends of the arms at each time step Combined with safe distance threshold The conflict probability at the corresponding time point is obtained. for: Then, the average conflict risk value of the entire candidate path is calculated. for: in, This represents the total number of discrete time steps.

[0038] Set conflict risk threshold And complete the risk level assessment: if the risk level is determined to be low, that is... Maintaining the initial weighting coefficients, calculate the path performance cost for each robotic arm; if it is determined to be a high-risk conflict area, i.e. This triggers an online adaptive weight adjustment mechanism. The main arm's weight coefficient remains unchanged, while only the auxiliary arm's weight coefficient is dynamically updated. The adjustment formula is as follows: in, The initial path length cost weight for the auxiliary arm. This is the weighting adjustment factor, ranging from 1.2 to 1.5. After adjustment, it must meet the following requirements. This temporarily increases the weighting coefficient of the obstacle avoidance cost of the auxiliary arm and decreases its path length cost weighting coefficient, making the auxiliary arm more proactive in yielding to the main arm. Then, based on the adjusted weighting coefficients... The performance cost of the auxiliary arm path is recalculated. Finally, considering the combined performance costs of both arm paths, the path combination with the optimal cost and no spatiotemporal conflict between arms is selected as the pre-feasible path. If the optimal path still has an extremely high risk of conflict, further investigation is conducted. In this case, the sampling node density is increased locally in the potential conflict area, and the path search and conflict determination are carried out again until a pre-feasible path that meets the requirements is matched, providing a basis for subsequent collision detection and path verification.

[0039] S4. Perform independent collision detection across the entire scene for the pre-feasible path output by S3. Add classification and judgment logic for dual-arm path planning results, clarify the processing rule that in single-arm failure scenarios, there is no need to return to S3, only perform local updates of the connected graph, and solidify the main and auxiliary arm judgment rules. For single-arm failure scenarios, perform successful path locking, failed path segment localization, and path correction operations based on local updates of the connected graph to completely eliminate various collision risks. The collision detection scope covers collisions between the robot and the ground, between the robot and obstacles, between non-adjacent joints of the robotic arm itself, and between the two robotic arms; define the space where the above objects are located as the fixed obstacle space, and the remaining area as the passable space.

[0040] When performing collision detection on the robotic arm between two path points, the jump path trajectory is first planned using the robot joint space interpolation method to avoid the robot exceeding the reachable range or encountering singularities. Then, based on the trajectory of the robotic arm's end effector, the links and joints of the robot body are projected into a three-dimensional grid space model using a forward kinematics model. Finally, based on the grid model positions of different objects, the collision is determined by grid overlap.

[0041] Let the set of grid cells corresponding to the space occupied by the body be . The set of grid cells occupied by fixed obstacle spaces is The collision determination quantification formula is: When C=1, it is determined that a collision has occurred on the corresponding robotic arm path; when C=0, it is determined that there is no collision on the path, and the collision detection results of robotic arms 1 and 2 and the specific collision path segment of the failed robotic arm are recorded independently.

[0042] In all scenarios, the S3 preset main and auxiliary arm rules remain unchanged. If the preset main arm path is successful, it will still be the main arm. If the preset main arm path fails but the auxiliary arm path is successful, the auxiliary arm with the successful path will be the temporary main arm, and the failed arm that needs to be corrected will be the temporary auxiliary arm.

[0043] like Figure 2 As shown, classification is performed based on the detection results. If both arm paths pass the detection, that is... =0 and If the value is 0, then the pre-feasible path is a collision-free feasible path, and proceed directly to S5; if both arm paths fail the detection, i.e. =1 and If the value is 1, then the connectivity matrix of the corresponding collision path is set to a disconnected state. The overall update formula for the connectivity matrix is: in, It is a connected Boolean matrix. For raster coordinates, The set of node pairs corresponding to the collision path is defined; the connected graph is updated by updating the connected matrix, and the pre-feasible paths of the two arms are searched again according to the differential evaluation function of S3 until the independent collision detection is passed.

[0044] If one arm path succeeds while the other fails, there's no need to return to S3 to search again. Instead, the successfully detected robotic arm path is locked, its spatial trajectory is projected onto the 3D grid model and marked as a temporary obstacle space, defining a set of temporary obstacle grids. The grid used to store the successful arm trajectory is updated using the following formula: in, This temporary obstacle, representing the set of grid cells occupied by the successful robot arm trajectory, only affects the replanning of the failed robot arm, while precisely locating the node pairs corresponding to the collision path segments of the failed robot arm. This involves setting the connectivity matrix of only the failed node pair to a disconnected state, thus achieving a local fine-grained update of the connected graph. The formula for the local update of the connectivity matrix is ​​as follows: This is to reduce computational redundancy.

[0045] Based on the locally updated connected graph, taking the previous valid node of the failed robot arm collision segment as the new starting point and the original target pose as the ending point, a new locally feasible path is directly searched in the locally connected graph. Full-scene collision detection is then performed again on the new path. At this point, collision determination must simultaneously avoid existing obstacles and the locked temporary obstacle space of the successful arm. The collision determination formula including temporary obstacles is: in, This indicates the collision determination situation involving temporary obstacles. The set of grid cells occupied by the robotic arm itself. A set of grid cells occupied by fixed obstacle spaces. To define a temporary obstacle grid set; when =0, perform dual-arm collaborative path collision detection between the locked successful path and the corrected failed arm path, when =1, only repeat the local update of the connectivity matrix and local path search operation for failed node pairs until the path merging verification is passed, ensuring the feasibility of the output path and the safety of the coordinated movement of the two arms.

[0046] S5. Perform high-precision trajectory optimization processing on collision-free feasible paths to generate continuous motion trajectories that match the dynamic characteristics of the robotic arm, and complete the conversion and output of standardized control commands.

[0047] The collision-free path obtained by the proposed algorithm only meets the requirement of no interference between spatial points, without considering dynamic indicators such as acceleration and deceleration smoothness, joint angular velocity constraints, and impact suppression during the movement of the robotic arm. If it is directly applied to actual dual-arm inspection robots, problems such as joint jitter, motion overshoot, and low trajectory tracking accuracy are likely to occur. Therefore, before actual deployment, the discrete path points need to be interpolated and fitted globally using the seventh-order polynomial interpolation method to generate a smooth motion trajectory that takes into account the continuity of position, velocity, acceleration, and jerk.

[0048] The seventh-order polynomial interpolation function uses time as the independent variable. By substituting the constraints of the position, velocity, acceleration, and jerk of the starting point, key intermediate points, and ending point of the path at a specified time, the coefficients of each polynomial are solved, thus determining the unique expression of the interpolation function. This achieves a high-order smooth fit to a discrete path. The core position interpolation function expression is as follows: in, For position functions, It is a velocity function. For acceleration function, For time, These are the polynomial coefficients.

[0049] The Cartesian space continuous pose trajectory optimized by seventh-order polynomial interpolation is mapped to the joint space continuous angle trajectory through the inverse kinematics model of S1. After discretizing the trajectory, it is encapsulated into a standardized control instruction package according to the robot control system communication protocol and sent to each joint servo driver according to the preset time step. The robot arm is precisely controlled in a closed loop to smoothly complete the inspection operation along the optimized path.

[0050] 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 person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered 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 method for obstacle avoidance path planning for a dual-arm robot based on a lazy probabilistic path graph algorithm, characterized in that, The method includes: Based on the constructed kinematic model of the dual robotic arms, the work space is discretized into an adaptive resolution grid and structured sampling is performed. The connection relationship of the grid structure is constructed based on the inertial connectivity determination. An initial global probability path map is constructed based on the set of sampling points and the connection relationship, and a connected graph is generated. Construct differentiated evaluation functions for the main arm and auxiliary arm, search for candidate path combinations of the two arms in the connected graph based on the differentiated evaluation functions, calculate the conflict risk of the candidate path combinations of the two arms, and select the path combination with the best cost and no inter-arm conflict as the pre-feasible path based on the calculation results; Independent collision detection is performed based on the pre-feasible path. If both arm paths pass the detection, the pre-feasible path is taken as a collision-free feasible path. If neither arm path passes the detection, the connected graph is updated and a new candidate path combination for both arms is searched based on the differential evaluation function. If one arm path is successfully detected while the other fails, the successfully detected robotic arm path is locked, the spatial trajectory of the successfully detected robotic arm path is projected into the grid and marked as a temporary obstacle space, the local connected graph is updated, and a local pre-feasible path is searched in the local connected graph to generate a collision-free feasible path. The collision-free feasible path is subjected to global interpolation fitting using the seventh-order polynomial interpolation method to obtain the continuous pose trajectory in Cartesian space and convert it into a robot control command package to realize obstacle avoidance path planning for the dual-arm robot.

2. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The independent collision detection includes: First, the jump path trajectory is planned using the robot joint space interpolation method; then, based on the trajectory of the robotic arm end effector, the links and joints of the robot body are projected into a three-dimensional grid space model using a forward kinematics model; finally, based on the grid positions of different objects, collisions are determined by grid overlap.

3. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 2, characterized in that, The quantization formula for determining whether a collision has occurred based on grid overlap is expressed as follows: in, Indicates the collision determination status. The set of grid cells corresponding to the space occupied by the main body. C represents the set of grid cells occupied by fixed obstacles; when C=1, it is determined that a collision has occurred on the corresponding robotic arm path; when C=0, it is determined that there is no collision on the path.

4. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The steps corresponding to the failure of both arm paths to pass the detection include: Set the connectivity matrix of the collision path to a disconnected state. The overall update formula for the connectivity matrix is: in, It is a connected Boolean matrix. For raster coordinates, The set of node pairs corresponding to the collision path is defined; the connectivity graph is updated by updating the connectivity matrix, and the pre-feasible paths of the two arms are searched again according to the differentiated evaluation function until the independent collision detection is passed.

5. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The steps for updating the locally connected graph include: Define a temporary obstacle grid set to store the grids occupied by the successful robot arm trajectory, locate the node pairs corresponding to the collision path segments of the failed robot arm, set the connectivity matrix of the node pairs to a disconnected state, and perform a local update on the connected graph, using the following formula: in, It is a connected Boolean matrix. This is to locate the node pairs corresponding to the collision path segments of the failed robotic arm.

6. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The steps corresponding to successful single-arm path detection and failed single-arm path detection also include: Based on the locally updated connected graph, taking the previous valid node of the failed robot arm collision segment as the new starting point and the original target pose as the ending point, a new locally pre-feasible path is searched in the locally connected graph. Collision detection is then performed again on the new locally pre-feasible path, and the determination formula is as follows: in, This indicates the collision determination situation involving temporary obstacles. The set of grid cells corresponding to the space occupied by the main body. A set of grid cells occupied by fixed obstacle spaces. To define a temporary obstacle grid set; when =0, perform dual-arm collaborative path collision detection between the locked successful path and the corrected failed arm path, when =1, repeat the local update of the connectivity matrix and local path search operation only for the failed node pairs until the merging verification is successful.

7. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The steps for calculating the conflict risk include: For each candidate path combination, a real-time conflict predictor is run, and the two candidate paths are discretized into spatiotemporal nodes with the same time step. The spatial Euclidean distance between the ends of the two arms at the time step is calculated, and the conflict probability at the corresponding time is obtained by combining the preset safety distance threshold. The average conflict risk value of the entire candidate path is then calculated to determine the conflict risk.

8. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 7, characterized in that, The cost-optimal screening step includes: Set a conflict risk threshold. If the average conflict risk value is less than the conflict risk threshold, keep the initial weight coefficients and calculate the path performance cost of the two robotic arms respectively. If the average conflict risk value is greater than the conflict risk threshold, an online adaptive weight adjustment mechanism is triggered. The main arm weight coefficient remains unchanged, and only the auxiliary arm weight coefficient is dynamically updated. Then, the auxiliary arm path performance cost is calculated based on the adjusted weight coefficient, the comprehensive dual-arm path performance cost is obtained, and the path combination with the optimal cost is selected.

9. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The step of converting the Cartesian space continuous pose trajectory into a joint space continuous angle trajectory through an inverse kinematics model, discretizing the joint space continuous angle trajectory, encapsulating the discretized joint space continuous angle trajectory into a standardized control instruction package according to the robot control system communication protocol, and sending it to each joint servo driver according to a preset time step.

10. The obstacle avoidance path planning method for a dual-arm robot based on a lazy probabilistic path graph algorithm according to claim 1, characterized in that, The steps of the global interpolation fitting include: The path point generation based on the collision-free feasible path takes into account position, velocity, acceleration, and jerk, resulting in a continuous and smooth motion trajectory. A seventh-order polynomial interpolation function is used with time as the independent variable. By substituting the position, velocity, acceleration, and jerk constraint polynomials of the starting point, intermediate key transition points, and ending point of the collision-free feasible path at a specified time, the coefficients of the polynomials are solved to obtain the interpolation function expression, thereby achieving a high-order smooth fit of the collision-free feasible path.

Images