Five-axis robot operation anti-collision method and system based on improved RRT algorithm

Abstract

The invention relates to the technical field of robots, and discloses a five-axis robot operation anti-collision method and system based on an improved RRT algorithm, and the system comprises an environment modeling module, a path planning module, a real-time monitoring module, a dynamic response module, an execution control module and a system optimization module. When the five-axis robot carries out closed cavity operation, the real-time distance relation between the robot and an obstacle can be continuously judged by constructing a cavity environment high-precision model and setting a minimum safety distance threshold value and utilizing environment dynamic data collected in real time and a collision detection algorithm at the same time; the system can sense the collision risk in the operation process in real time, collision accidents caused by the uncertainty of the environment of the robot are avoided, and the operation safety and reliability are guaranteed.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to a collision avoidance method and system for five-axis robot operations based on an improved RRT algorithm. Background Technology

[0002] A five-axis robot is a widely used industrial robot with five degrees of freedom. With the continuous development of robotics technology, more and more robots are beginning to replace humans in performing various tasks. Robot is a common term for automatically controlled machines, which include all machines that simulate human behavior or thought, as well as those that mimic other living organisms. In modern industry, a robot refers to an artificial machine device that can automatically perform tasks to replace or assist human work.

[0003] Currently, when five-axis robots are used in complex environments with enclosed cavities, due to incomplete prior environmental information and dynamic uncertainties, traditional path planning algorithms cannot perceive the relative positional changes between the internal structure of the cavity and the robot body in real time. When the robot moves too close to the cavity wall and internal obstacles, it will cause collision risks, resulting in robot damage and task interruption.

[0004] Existing patent CN119017395B discloses a path planning and replanning method for robotic arms in dynamic medical scenarios. During replanning, it combines the RRT* algorithm with a virtual potential field, setting a scaling factor to change the weighting influence of the RRT* algorithm and the virtual potential field during expansion. This reduces the arbitrariness in RRT* expansion and the potential local optima problem caused by the virtual potential field to some extent. It acquires environmental point cloud information through a depth camera, tracks static and dynamic obstacles, and adjusts the path in real time to avoid them. However, this method is not optimized for the characteristics of a five-axis robot, which results in the following issues: The lack of processing for the range of motion of asymmetric joints can easily lead to paths not meeting physical limits; the replanning mechanism does not preset a strict time window, resulting in a lag in dynamic obstacle avoidance response; the safety threshold setting in environmental modeling is static and does not introduce dynamic compensation for sensor noise; and the lack of integration of multi-source sensor data fusion results in insufficient system robustness. These defects make it difficult to adapt to the high precision and real-time requirements of closed cavity operations.

[0005] Existing patent CN116352714B discloses a method for obstacle avoidance path planning for a robotic arm. This method uses virtual joint interpolation to interpolate the links between adjacent joints, and then uses a spherical bounding box to enclose the robotic arm to simplify the robotic arm model, while also simplifying the distance calculation in the obstacle avoidance process. It employs the RRT-Connect algorithm for global path planning in a static environment, which can quickly obtain a feasible path. By controlling the alternating growth of random trees, the directionality of the path is improved. An improved artificial potential field method, by introducing guiding force and distance influence factors, can effectively avoid problems such as stopping and shaking caused by getting trapped in local minima, and target unreachability caused by repulsive forces near the target point, enabling the robotic arm to move normally to the target point. However, this method has shortcomings in terms of environmental adaptability, which are as follows: The path planning is not optimized for the narrow structure of the closed cavity, and relies on a simplified model of a general spherical bounding box, resulting in a high collision risk. The collision detection is based on instantaneous distance judgment and does not adopt a multi-cycle continuous monitoring mechanism, resulting in a high false alarm rate. The local obstacle avoidance does not verify the continuity of path connection acceleration, which can easily cause sudden changes in joint torque. The system design is not modular, lacks edge computing support, and has low resource allocation efficiency.

[0006] Therefore, a collision avoidance method and system for five-axis robot operations based on an improved RRT algorithm is proposed to solve the above problems. Summary of the Invention

[0007] To address the shortcomings of existing technologies, this invention provides a collision avoidance method and system for five-axis robots based on an improved RRT algorithm. This solves the problem mentioned in the background that when a robot moves too close to the cavity wall and internal obstacles, it may cause collision risks, resulting in robot damage and task interruption.

[0008] To achieve the above objectives, the present invention provides the following technical solution: a collision avoidance method for five-axis robots based on an improved RRT algorithm, comprising the following steps: S1. Environmental Modeling and Safety Threshold Setting: Environmental data of the enclosed cavity is collected using a 3D point cloud scanning device. A parametric modeling technique is then used to construct the enclosed cavity environment model, and a minimum safe distance threshold between the robot and the cavity wall is set within the model. ; S2. Initial Collision-Free Path Planning; Based on the enclosed cavity environment model and the minimum safe distance threshold, an improved RRT algorithm is used to generate an initial collision-free path; the improved RRT algorithm includes: S21. Initialize the parameters of the improved RRT algorithm, including setting the random tree expansion step size, maximum number of iterations, and target point bias probability; S22. A bidirectional RRT expansion strategy is adopted, and random trees are grown simultaneously from the robot's initial configuration and target configuration. When the two trees are expanded to the end pose error is less than a set threshold, the path is determined to be connected. S23. During the growth of the random tree, a collision particle acceleration expansion mechanism is introduced: multiple collision particles are uniformly sampled on the current path, and each particle is classified into safe particles, deceleration particles and dangerous particles according to the real-time distance between each particle and the obstacle, and the expansion step size and priority direction of the random tree are dynamically adjusted accordingly. S24. During the random tree growth process, joint space constraints are introduced: the asymmetric rotation angle range of the third axis of the five-axis robot is truncated to ensure that the new node meets the physical limits of the joint. S25. During the path smoothing stage, Bézier curves are used to fit discrete path points so that the attitude change rate of the end effector meets the requirements of continuous operation. The random tree expansion process is defined by the following formula: ; in, For the newly generated tree node, The nearest node in the current random tree. For random sampling points, This is the step size coefficient; S3. Real-time motion monitoring and collision risk assessment: The robot operates along the generated initial collision-free path, and real-time environmental dynamic change data is collected during the operation. The collision detection algorithm determines whether the real-time distance between the robot and the cavity obstacle is less than the minimum safe distance threshold. If the distance is detected to be too close in multiple consecutive sampling cycles, a collision risk is determined. S4. When a collision risk is detected, a dynamic replanning mechanism is triggered: the closed cavity environment model is updated based on real-time environmental data, and a locally optimized path is generated within a preset time window using an improved RRT algorithm to replace the risky section in the original path. S5, Path Smoothing and Execution Control: The generated local optimized path is converted into control commands and sent to the five-axis robot control unit to drive the joint motors to perform continuous obstacle avoidance operations.

[0009] Another objective of this invention is to provide a five-axis robot collision avoidance system based on an improved RRT algorithm, for implementing the five-axis robot collision avoidance method described above, comprising: The environment modeling module is configured to: receive environmental data collected by a 3D point cloud scanning device, construct a closed cavity environment model, and set a minimum safe distance threshold; The path planning module includes an improved RRT algorithm unit and a kinematic constraint processing unit. The path planning module is configured to: receive the closed cavity environment model and minimum safe distance threshold output by the environment modeling module; generate an initial collision-free path through the improved RRT algorithm unit; and output path data adapted to the five-axis robot structure through the kinematic constraint processing unit. The real-time monitoring module includes a multi-source sensor acquisition unit and a collision detection algorithm unit. The real-time monitoring module is configured to: receive path data output by the path planning module, acquire robot pose and environmental dynamic data through the multi-source sensor acquisition unit, determine collision risk using the collision detection algorithm unit, and output a risk warning signal. The dynamic response module includes an environment model update unit, a local path replanning unit, and a path smoothing processing unit. The dynamic response module is configured to: receive the risk warning signal output by the real-time monitoring module, refresh the cavity model data through the environment model update unit, generate an optimized path through the local path replanning unit, and output the sub-path after connection verification through the path smoothing processing unit. The execution control module includes a path-command conversion unit and an exception handling unit. The execution control module is configured to: receive sub-path data output by the dynamic response module, generate motor control signals through the path-command conversion unit, drive the five-axis robot joints to perform operations, and output status data, including real-time joint pose, torque load and abnormal alarm information, and manage emergency retraction and alarm processes through the exception handling unit. The system optimization module includes a resource allocation unit and a performance monitoring unit. The system optimization module is configured to: receive environmental dynamic data output by the real-time monitoring module and status data output by the execution control module; calculate the execution efficiency of the optimization algorithm by the resource allocation unit; and have the performance monitoring unit feed back the data to the environment modeling module and the path planning module to adjust the processing parameters.

[0010] Compared with existing technologies, this invention provides a collision avoidance method and system for five-axis robots based on an improved RRT algorithm, which has the following beneficial effects: (1) In this invention, when the five-axis robot performs closed cavity operations, by constructing a high-precision model of the cavity environment and setting a minimum safe distance threshold, and by using real-time collected environmental dynamic data and collision detection algorithms, the robot can continuously judge the real-time distance relationship between the robot and obstacles, so that the system can perceive the collision risk in the operation process in real time, avoid collision accidents caused by environmental uncertainty, and ensure the safety and reliability of the operation.

[0011] (2) In the present invention, during the robot operation path planning process, the asymmetric joint range of motion limit and end effector posture constraint are incorporated into the five-axis dedicated improved RRT algorithm, and optimization processing is carried out in the path smoothing stage, so that the generated path can not only adapt to the physical structure characteristics of the five-axis robot and avoid joint over-limit operation, but also ensure the continuity and smoothness of the end effector movement, thereby improving the path execution accuracy and equipment service life.

[0012] (3) In this invention, when dynamic obstacles and collision risks are detected, a real-time trajectory prediction and dynamic replanning mechanism is activated to quickly generate a local optimized path based on the latest environmental data and verify its continuity. This enables the system to respond to environmental changes and complete collision-free path planning in a short time, solving the problem of response lag when dealing with dynamic obstacles in traditional methods and enhancing the five-axis robot's operational adaptability and obstacle avoidance success rate in complex dynamic environments. Attached Figure Description

[0013] Figure 1 This is a flowchart of the collision avoidance method for five-axis robot operation based on the improved RRT algorithm of the present invention; Figure 2 This is a schematic diagram of the collision avoidance system for a five-axis robot based on the improved RRT algorithm of this invention. Detailed Implementation

[0014] 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 embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0015] For specific implementation examples, please refer to: Figure 1-2 A collision avoidance method for five-axis robots based on an improved RRT algorithm includes the following steps: S1. Collect environmental data of the closed cavity using a 3D point cloud scanning device, construct a closed cavity environment model using parametric modeling technology, and set the minimum safe distance threshold between the robot and the cavity wall in the model.

[0016] Step S1 involves constructing a closed cavity environment model and setting a minimum safe distance threshold, specifically including the following operations: S11. Use a combination of lidar and depth camera to scan the internal structure of the closed cavity to obtain high-precision three-dimensional point cloud data; S12. Convert the point cloud data into a parametric mesh in the closed cavity environment model using a surface reconstruction algorithm, and mark the cavity obstacle boundaries on the mesh surface; S13. Based on the dimensions of the five-axis robot body and the outer dimensions of the working tool, set the minimum safe distance threshold between the robot and the boundaries of each obstacle in the mesh model. This includes the following steps: S131. Robot envelope voxelization: Based on the robot's CAD model, voxelization segmentation is performed in the cavity environment coordinate system to generate a set of robot shell envelope voxels. voxel resolution Based on the robot's motion accuracy settings; S132. Construction of the obstacle surface distance field: Calculate the directed distance field SDF for the cavity obstacle mesh model, and generate the distance mapping function from any point on the obstacle surface to the nearest obstacle: SDF ; in, Let be the coordinates of any point in space. Let the coordinates be those of a point on the surface of the obstacle. Let be the set of points on the surface of the obstacle; S133, Calculation of minimum distance between envelope voxels, traversal All voxel center points Calculate the minimum Euclidean distance from it to the obstacle: ; in, For the first The distance from the center point of an individual element to the obstacle. For the first The coordinates of the center point of an individual element; The minimum value of all voxel distances is taken as the theoretical safe distance: ; in, The theoretical minimum safe distance is denoted by , and the distance values ​​for all voxels are given. Find the minimum value in the middle. For indexing; S134, Threshold dynamic compensation setting, based on robot motion control error. and sensor noise Perform dynamic compensation: ; in, The compensation coefficient is the final set safety distance threshold. , Obtained through robot calibration experiments To control error, This refers to sensor error.

[0017] S2. Based on the closed cavity environment model and the minimum safe distance threshold, an improved RRT algorithm optimized for five-axis robots is used to generate an initial collision-free path. The improved RRT algorithm incorporates the kinematic constraints of five-axis heterogeneous SCARA robots, including asymmetric joint range of motion restrictions and end effector posture constraints, to ensure that the path fully adapts to the five-axis structural characteristics and redundant degrees of freedom.

[0018] Step S2 uses an improved RRT algorithm to generate an initial collision-free path, specifically including the following operations: S21. Initialize the parameters of the improved RRT algorithm, including setting the random tree expansion step size, maximum number of iterations, and target point bias probability; S22. A bidirectional RRT expansion strategy is adopted, which simultaneously grows random trees from the robot's initial configuration and target configuration. When the two trees expand to the end pose error is less than a set threshold, the path is determined to be connected, so as to improve the planning efficiency in narrow cavities. S23. During the growth of the random tree, a collision particle acceleration expansion mechanism is introduced: multiple collision particles are uniformly sampled on the current path, and each particle is classified into safe particles, deceleration particles and dangerous particles according to the real-time distance between each particle and the obstacle, and the expansion step size and priority direction of the random tree are dynamically adjusted accordingly. S24. During the random tree growth process, joint space constraints are introduced: the asymmetric rotation angle range of the third axis of the five-axis heterogeneous SCARA robot is truncated to ensure that the new node meets the joint physical limits. The boundary truncation is implemented as follows: S241, Node Sampling: Random sampling is performed in the joint space to obtain a sample point containing five joint angles: ; in, The five-dimensional joint angle vector is obtained through random sampling. , , , These represent the sampling angles of the first, second, fourth, and fifth joints, respectively. The angle value of the third joint was obtained through random sampling; S242. Boundary Truncation Judgment and Handling: Query the allowable asymmetric working range of the robot's third joint. ,in As the lower limit, The upper limit is set for the random sampled values. Cut off the angle to the valid range according to the following rules to obtain a valid new angle. : ; S243. Generate a valid new node: The truncated angle... replace This constitutes a valid new sample point: ; The algorithm will then continue to perform expansion operations based on this new sample point; S25. During the path smoothing stage, Bézier curves are used to fit discrete path points so that the attitude change rate of the end effector meets the requirements of continuous operation. The random tree expansion process is defined by the following formula: ; Where m For the newly generated tree node, The nearest node in the current random tree. For random sampling points, This is the step size coefficient.

[0019] S3. During the operation of the robot along the generated initial collision-free path, real-time dynamic change data of the environment is collected, and the collision detection algorithm is used to determine whether the real-time distance between the robot and the cavity obstacle is less than the minimum safe distance threshold.

[0020] Step S3 uses a collision detection algorithm to determine the real-time distance between the robot and obstacles in the cavity, specifically including the following operations: S31. Install inertial measurement units on the joints of the five-axis robot to collect the position of each joint and the pose of the end effector in real time. S32. Map the real-time pose data to the cavity environment model and calculate the minimum distance between the outer shell envelope of the five-axis robot and the surface of the obstacle mesh. S33. If the minimum distance is below the safety threshold for two consecutive sampling periods, it is determined to be a collision risk event. The minimum distance is calculated using the following formula: ; in, Let be the minimum Euclidean distance between the robot and the obstacle, and be the set of points on the robot's outer surface. For the point set of the obstacle mesh surface, For points on the surface of the robot's outer shell, Points on the surface of the obstacle mesh; The determination of a collision risk event is carried out in accordance with the following steps: S331. Sampling period synchronization setting: based on the main control period of the robot control system. Maximum refresh cycle of environmental perception sensors The larger of the two values ​​is taken as the system reference period. : ; Set the sampling period for collision detection for integers Times: ; S332, Continuous threshold determination logic: at each sampling time Calculate the current minimum distance Set a length of First-in, first-out queue Store the most recent Distance determination results for each sampling period; Each element in the queue The value of is determined by the following formula: ; in, For safe distance threshold, Let i be the minimum distance value measured at time i, where i is the index number; When queue All When all elements are 1, it is determined that the value is continuously below the safety threshold, triggering a collision risk event.

[0021] S4. When a collision risk is detected, a dynamic replanning mechanism is triggered: the closed cavity environment model is updated based on real-time environmental data, and a locally optimized path is generated within a preset time window using an improved RRT algorithm to replace the risky section in the original path.

[0022] Step S4 triggers the dynamic replanning mechanism to generate a locally optimized path, which specifically includes the following operations: S41. After a collision risk event is triggered, freeze the current path execution instructions and save a snapshot of the five-axis robot's state at the moment of environmental change. S42. Starting from the state snapshot, the improved RRT algorithm is called within the local space to generate obstacle avoidance sub-paths. The sub-path planning time window is less than 100ms, which is suitable for the real-time control requirements of five axes. This process is implemented according to the following steps: S421. State Snapshot Analysis and Local Space Construction: Read the robot's current pose information and joint velocity data contained in the state snapshot. Centered on the current position, define a local planning space based on the robot's kinematic limits and a 100-millisecond time constraint. The range of this space is determined by the product of the robot's maximum linear velocity at the end of the robot and the time window. S422, Improved RRT algorithm for directional initialization: Set the random tree root node to the current position; Define the local target region: Generate a cone-shaped target sampling region in the local space with the direction of movement as the main axis; Adjusting algorithm parameters: With the goal of satisfying the planning time window constraints, reducing the maximum number of iterations and the expansion step size; S423, Real-time Collision Detection Integration: During the random tree expansion process, for each new node generated, the minimum distance from the node to environmental obstacles is calculated, and the collision risk judgment logic is called. If the minimum distance is consistently lower than the safety threshold, the node is discarded and the expansion is restarted. S424. Path Extraction and Optimization: After reaching the required number of iterations and successfully connecting the target region, extract the local path from the root node to the target point, and use a path smoothing algorithm to smooth the path to ensure continuity with the tangent direction of the global path.

[0023] S425, Calculation time constraint verification: Monitor the entire process time. If it exceeds 85 milliseconds, trigger the acceleration mechanism: Improve the calculation efficiency by reducing the resolution of the environmental point cloud and shrinking the local planning space, and ensure that the total planning time of the final output path is less than 100 milliseconds. S43. Verify the acceleration continuity at the junction of the sub-path and the global path. If the joint torque exceeds the limit due to a sudden change, replan. The following logic is used in S43 to verify the acceleration continuity between the sub-path and the global path: S431. Extract the joint velocity vectors of the sub-path start point and the global path end point; S432. The theoretical acceleration value of the path connection point is calculated using the finite difference method. The specific process is as follows: S4321, Velocity Vector Extraction: The obtained subpath starting joint velocity vector is denoted as... The velocity vector of the joint at the end of the global path is denoted as... The theoretical velocity vector at the connection point Pick value; S4322, Time Step Setting: Obtain the control cycle of the motion control system. Define the time step for difference computation. ,Pick Integer multiples of; S4323, Difference Calculation: Theoretical acceleration is calculated using the first-order backward difference method. ; in, This is the theoretical joint acceleration vector. The joint velocity vector at the starting point of the sub-path. The time step used for the difference calculation; S4324. Output the results: Output the calculated... Output to S433 for torque over-limit detection; S433. When the theoretical acceleration value exceeds the maximum allowable value of the five-axis robot joint servo system, a uniform speed transition section is inserted at the connection point.

[0024] S5. The generated local optimized path is converted into control commands and sent to the five-axis robot control unit to drive the joint motors to perform continuous obstacle avoidance operations.

[0025] Step S5, which converts the local optimization path into control instructions, also includes an exception handling mechanism: S51. When the cumulative number of dynamic replanning failures exceeds the limit, control the five-axis robot to perform an emergency retraction action to the nearest safe pose. S52, trigger the audible and visual alarm device and upload abnormal environmental data to the remote monitoring platform.

[0026] The five-axis robot collision avoidance system based on the improved RRT algorithm includes an environment modeling module, a path planning module, a real-time monitoring module, a dynamic response module, an execution control module, a system optimization module, and a system deployment edge computing unit.

[0027] The environment modeling module is used to receive environmental data collected by the 3D point cloud scanning device, construct a closed cavity environment model through the parametric modeling unit, and output the minimum safe distance threshold through the safety threshold setting unit.

[0028] The path planning module receives the closed cavity environment model and minimum safe distance threshold output by the environment modeling module, generates an initial collision-free path through the five-axis dedicated improved RRT algorithm unit, and outputs path data adapted to the five-axis robot structure through the kinematic constraint processing unit.

[0029] The real-time monitoring module receives path data output from the path planning module, acquires robot pose and environmental dynamic data through a five-axis multi-source sensor acquisition unit, determines collision risk using a collision detection algorithm unit, and outputs a risk warning signal. The multi-source sensor acquisition unit is configured to receive: 3D point cloud data of the cavity acquired by a 2D LiDAR, real-time obstacle distance data acquired by an ultrasonic sensor, joint load data acquired by a joint torque sensor, contact force data acquired by a six-dimensional force sensor, and link temperature data acquired by a temperature sensor. Kalman filtering is used to achieve time synchronization of multi-source data, the 3σ criterion is used to eliminate interference signals, and a confidence-weighted approach is used, with a LiDAR weight of 0.8 and an ultrasonic weight of 0.2, to construct a static-dynamic dual-layer obstacle map.

[0030] The 2D lidar has a ranging range of 0.1 to 12 meters, an accuracy of ±2%, and a sampling frequency of 10 Hz. The ultrasonic sensor has a ranging range of 5-500 mm, a resolution of 1 mm, and a sampling frequency of 100 Hz. The joint torque sensor has a range of ±50 N·m, an accuracy of ±0.1%, and a sampling frequency of 1 kHz. The six-dimensional force sensor has a range of ±100N / ±5N・m, an accuracy of ±0.5%, and a sampling frequency of 1kHz. The temperature sensor has a range of -40 to 125℃, an accuracy of ±0.1℃, and a sampling frequency of 10Hz.

[0031] The real-time monitoring module integrates multi-source sensing units.

[0032] The dynamic response module is used to receive the risk warning signal output by the real-time monitoring module, refresh the cavity model data through the environmental model update unit, generate an optimized path through the local path replanning unit, and output the connected and verified sub-path through the path smoothing processing unit.

[0033] The dynamic response module incorporates a replanning priority strategy: when a temporary obstacle is detected as a moving object, a real-time trajectory prediction algorithm is activated to optimize the replanning input. The real-time trajectory prediction algorithm optimizes the implementation of replanning and is called and integrated by the local path replanning unit, including the following steps: S100, Dynamic Data Acquisition: The position sequence and velocity vector of the moving obstacle are acquired in real time through the multi-source sensing unit of the real-time monitoring module; S200, Motion State Modeling: Construct kinematic equations based on the rate of change of obstacle displacement to predict its trajectory within the replanning time window; S300, Trajectory Optimization Output: The predicted trajectory is mapped to the dynamic obstacle boundary and input into the local path replanning unit of the improved RRT algorithm to generate obstacle avoidance sub-paths.

[0034] The execution control module receives sub-path data output by the dynamic response module, generates motor control signals through the path-command conversion unit, drives the five-axis robot joints to perform tasks, and manages emergency retraction and alarm processes through the exception handling unit. This execution control module achieves closed-loop logic of force-position hybrid control, thermal deformation compensation, and collision emergency response through multi-sensor feedback. The specific control mechanism is as follows: (1) Force-position hybrid control.

[0035] Position control: The end effector moves along the path planned by the improved RRT and is adjusted by the robot controller PID, with a path tracking error of <0.05mm;

[0036] Force control: When the end effector contacts the workpiece, including during bolt assembly and wafer gripping, the contact force is fed back through a six-dimensional force sensor, and the displacement of the translation axis is adjusted to achieve closed-loop control of the contact force with a control accuracy of ±2N.

[0037] (2) Thermal deformation compensation.

[0038] Localized high temperatures within a closed cavity can easily lead to thermal deformation of the connecting rod. The compensation logic is as follows: ; in, This is the thermal deformation error vector. The current temperature. Based on room temperature, The temperature-error coefficient matrix is ​​3×4. Through five-point experimental fitting at 25℃, 40℃, 60℃, 80℃, and 100℃, the error coefficient of a certain RRRRP robot is calculated. Matrix example: ; Compensation method: As a correction for improving the RRT path, θ2 and θ3 are adjusted through the redundant degree of freedom optimization module to offset the thermal deformation error, and the end error after compensation is <0.02mm; (3) Collision emergency response.

[0039] Based on real-time feedback from joint torque sensors, a two-stage collision response mechanism is set up: Soft collision warning: A sudden change in torque at a joint of 10% to 20% triggers local replanning (RRT) and simultaneously reduces the robot's motion speed to 70%. Hard collision, emergency: A sudden change in torque at a certain joint >20%, the robot stops moving within 10ms, the end effector retreats 5mm, and the global path is replanned.

[0040] The system optimization module receives environmental dynamic data output by the real-time monitoring module and status data output by the execution control module. It calculates the efficiency of the optimization algorithm by the resource allocation unit and feeds it back to the environment modeling module and path planning module by the performance monitoring unit to adjust the processing parameters. At the same time, the system optimization module is also configured to perform redundant degree of freedom optimization. For one redundant degree of freedom of multi-axis robots such as RRRRP, the rotation angles θ2 and θ3 of the R2 and R3 joints are selected as optimization variables. Since θ1 affects the X / Y position of the end effector and θ4 affects the yaw posture, the adjustment of the intermediate joints is more flexible. A multi-objective optimization model is constructed to balance obstacle avoidance safety and operation performance.

[0041] Multi-objective optimization function: Using θ2 and θ3 as optimization variables, it integrates three major objectives: collision avoidance, joint limits, and torque balance. The function expression is as follows: ; In the formula: For collision avoidance target terms, weights , , The minimum distance between the i-th link and the obstacle; For the joint limit objective term, the weights are... , , Let i be the midpoint of the stroke of joint i; For the torque balance objective term, the weights are... , , Let be the real-time torque of joint i. The average torque; The optimized solution algorithm employs an improved particle swarm optimization algorithm to solve the above multi-objective optimization function, balancing speed and accuracy. Algorithm parameters: number of particles 30, number of iterations 20, inertia weight 0.7, learning factor ; Constraints: End-effector pose error < 0.01 mm, Yaw angle error around the Z-axis < 0.05°, joint rotation angle ∈ ±90°; Solution efficiency: On an industrial PC, the single solution time is <1ms, which meets the requirements of real-time control.

[0042] The system deploys edge computing units to perform the following collaborative mechanisms: S101. Implement parallel compression processing of point cloud data in the environment modeling module; S102. In the path planning module, GPU acceleration is used to improve the random tree expansion process of the RRT algorithm; S103. Reserve real-time bandwidth for the dynamic response module to ensure that the delay in generating replanning instructions is less than 50ms; S104 provides a computing resource scheduling interface for the system optimization module, dynamically allocating computing power to optimize the overall system performance.

[0043] The operation steps of the five-axis robot collision avoidance method and system based on the improved RRT algorithm are as follows: Step 1: Environment modeling and safety threshold setting.

[0044] Environmental data of the enclosed cavity was collected using a 3D point cloud scanning device, and a high-precision enclosed cavity environment model was constructed using parametric modeling technology. In this model, based on the physical dimensions of the robot body and the end effector, minimum safe distance thresholds were set between the robot and the cavity walls and various obstacles, providing a judgment benchmark for subsequent collision detection.

[0045] Step 2: Initial collision-free path planning.

[0046] Based on the established environment model and safety thresholds, an improved RRT algorithm is used for global path planning. During the random tree growth process, this algorithm deeply integrates the kinematic constraints of the five-axis heterogeneous SCARA robot, including asymmetric joint range of motion restrictions and end effector posture constraints, thereby generating an initial working path that is collision-free in both geometric and joint spaces and conforms to the robot's structural characteristics.

[0047] Step 3: Real-time motion monitoring and collision risk assessment.

[0048] During the robot's operation along the initial path, sensors integrated into the joints and end effector collect its pose data and information on dynamic environmental changes in real time. A collision detection algorithm continuously calculates the minimum distance between the robot's outer shell envelope and the obstacle mesh surface, and determines whether this distance remains below a safe threshold. If the distance is detected to be too close for multiple consecutive sampling cycles, a collision risk is determined.

[0049] Step 4: Dynamic local path replanning.

[0050] Once a collision risk is detected, a dynamic replanning mechanism is immediately triggered. Starting with a snapshot of the current state, the system quickly invokes an improved Replanning Time (RRT) algorithm within a local space. This process integrates real-time environmental data and generates a locally optimized sub-path that avoids the current risk area within a short time window, replacing the dangerous sections in the original path.

[0051] Step 5: Path smoothing and execution control.

[0052] The local sub-paths obtained from replanning are smoothed and optimized to ensure that their pose and acceleration are continuous with the global path at the connection points, avoiding abrupt changes in joint torque. Finally, the optimized path commands are sent to the control unit of the five-axis robot to drive the joint motors to perform continuous and stable obstacle avoidance actions, thereby completing a closed-loop collision prevention process from perception and decision-making to execution.

[0053] This embodiment of the five-axis robot collision avoidance method and system based on the improved RRT algorithm mainly solves the following three major technical problems: (1) Collision risk caused by environmental uncertainty in the operation of five-axis robots in closed cavities. Traditional methods are prone to path misfitting and response delay because they do not consider dedicated kinematic constraints and real-time dynamic replanning; (2) The problem of obstacle avoidance lag caused by dynamic obstacles is avoided by time window constraints and acceleration continuity verification; (3) The problem of insufficient system reliability is improved by modular design and abnormal handling mechanism, which enhances the adaptability in complex environments.

[0054] The innovation of the five-axis robot collision avoidance method and system based on the improved RRT algorithm in this embodiment lies in the improved RRT algorithm and the fully closed-loop system designed specifically for five-axis robot operation in a closed cavity. The innovation focuses on the following three points: (1) The improved RRT algorithm is deeply integrated with kinematic constraints, including the boundary truncation of the asymmetric angle of the third axis, to ensure that the path fits the physical structure; (2) The real-time monitoring module integrates 100ms time window replanning through multi-source sensing to achieve dynamic obstacle avoidance and fast response; (3) The modular system architecture combined with edge computing GPU acceleration optimizes resource scheduling.

[0055] The technical effects achieved by the five-axis robot collision avoidance method and system based on the improved RRT algorithm in this embodiment include: (1) Improved operation safety and accuracy. Through high-precision environmental modeling and real-time collision detection, the success rate of collision avoidance is improved, ensuring the safety of the robot and the environment; (2) Optimized path planning and extended equipment life. The improved RRT algorithm ensures smooth and continuous path, reduces joint wear, and improves equipment life; (3) Enhanced dynamic response efficiency. The replanning mechanism completes the generation of local paths in a short time, and the response speed is improved compared with the traditional method, solving the lag problem; (4) Optimized overall system performance. Edge computing support improves resource utilization, and anomaly handling further reduces the risk of failure.

[0056] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0057] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A collision avoidance method for five-axis robots based on an improved RRT algorithm, characterized in that, Includes the following steps: S1. Environmental Modeling and Safety Threshold Setting: Environmental data of the enclosed cavity is collected using a 3D point cloud scanning device. A parametric modeling technique is then used to construct the enclosed cavity environment model, and a minimum safe distance threshold between the robot and the cavity wall is set within the model. ; S2, Initial collision-free path planning; Based on the closed cavity environment model and the minimum safe distance threshold, an improved RRT algorithm is used to generate an initial collision-free path; The improved RRT algorithm includes: S21. Initialize the parameters of the improved RRT algorithm, including setting the random tree expansion step size, maximum number of iterations, and target point bias probability; S22. A bidirectional RRT expansion strategy is adopted, and random trees are grown simultaneously from the robot's initial configuration and target configuration. When the two trees are expanded to the end pose error is less than a set threshold, the path is determined to be connected. S23. During the growth of the random tree, a collision particle acceleration expansion mechanism is introduced: multiple collision particles are uniformly sampled on the current path, and each particle is classified into safe particles, deceleration particles and dangerous particles according to the real-time distance between each particle and the obstacle, and the expansion step size and priority direction of the random tree are dynamically adjusted accordingly. S24. During the random tree growth process, joint space constraints are introduced: the asymmetric rotation angle range of the third axis of the five-axis robot is truncated to ensure that the new node meets the physical limits of the joint. S25. During the path smoothing stage, Bézier curves are used to fit discrete path points so that the attitude change rate of the end effector meets the requirements of continuous operation. The random tree expansion process is defined by the following formula: ； in, For the newly generated tree node, The nearest node in the current random tree. For random sampling points, This is the step size coefficient; S3. Real-time motion monitoring and collision risk assessment: The robot operates along the generated initial collision-free path, and real-time environmental dynamic change data is collected during the operation. The collision detection algorithm determines whether the real-time distance between the robot and the cavity obstacle is less than the minimum safe distance threshold. If the distance is detected to be too close in multiple consecutive sampling cycles, a collision risk is determined. S4. When a collision risk is detected, a dynamic replanning mechanism is triggered: the closed cavity environment model is updated based on real-time environmental data, and a locally optimized path is generated within a preset time window using an improved RRT algorithm to replace the risky section in the original path. S5, Path Smoothing and Execution Control: The generated local optimized path is converted into control commands and sent to the five-axis robot control unit to drive the joint motors to perform continuous obstacle avoidance operations.

2. The collision avoidance method for five-axis robot operation based on the improved RRT algorithm according to claim 1, characterized in that, In step S1, the construction of the enclosed cavity environment model and the setting of the minimum safe distance threshold include the following steps: S11. Use a combination of lidar and depth camera to scan the internal structure of the closed cavity to obtain high-precision three-dimensional point cloud data; S12. The three-dimensional point cloud data is converted into a parametric mesh in the closed cavity environment model through the surface reconstruction algorithm, and the boundary of the cavity obstacle is marked on the mesh surface to obtain the mesh model. S13. Based on the dimensions of the five-axis robot body and the outer dimensions of the working tool, set the minimum safe distance threshold between the robot and the boundaries of each obstacle in the mesh model; the setting of the minimum safe distance threshold includes: S131. Based on the robot's CAD model, perform voxel segmentation in the cavity environment coordinate system to generate a set of robot shell envelope voxels. voxel resolution Based on the robot's motion accuracy settings; S132. Calculate the directed distance field SDF for the cavity obstacle mesh model, and generate the distance mapping function from any point on the obstacle surface to the nearest obstacle: SDF ; in, Let be the coordinates of any point in space. Let the coordinates be those of a point on the surface of the obstacle. Let be the set of points on the surface of the obstacle; S133, Traversal All voxel center points Calculate the minimum Euclidean distance from it to the obstacle: ； in, For the first The distance from the center point of an individual element to the obstacle. For the first The coordinates of the center point of an individual element; The minimum value of all voxel distances is taken as the theoretical minimum safe distance. : ； S134. Based on robot motion control error and sensor noise Dynamic compensation is performed to obtain the final set minimum safe distance threshold. : ； in, For compensation coefficient, To control error, This refers to sensor error.

3. The collision avoidance method for five-axis robot operations based on the improved RRT algorithm according to claim 1, characterized in that, In S24, the boundary truncation process includes: S241. Random sampling is performed in the joint space to obtain a sample point containing five joint angles: ； in, The five-dimensional joint angle vector is obtained through random sampling. , , , These represent the sampling angles of the first, second, fourth, and fifth joints, respectively. The angle value of the third joint was obtained through random sampling; S242. Boundary Truncation Judgment and Handling: Query the allowable asymmetric working range of the robot's third joint. ,in As the lower limit, The upper limit is set for the random sampled values. Cut off the angle to the valid range according to the following rules to obtain a valid new angle. : ； S243. Generate a valid new node: The truncated angle... replace This constitutes a valid new sample point: ； S244. Continue the expansion operation based on the new sample points.

4. The collision avoidance method for five-axis robot operation based on the improved RRT algorithm according to claim 1, characterized in that, In step S3, the collision detection algorithm includes the following steps: S31. Install inertial measurement units on the joints of the five-axis robot to collect the position of each joint and the pose of the end effector in real time. S32. Map the real-time pose data to the closed cavity environment model and calculate the minimum distance between the five-axis robot's outer shell envelope surface and the obstacle mesh surface. The minimum distance calculation is defined by the following formula: ； in, Let be the minimum Euclidean distance between the robot and the obstacle, and be the set of points on the robot's outer surface. For the point set of the obstacle mesh surface, For points on the surface of the robot's outer shell, Points on the surface of the obstacle mesh; S33. If the minimum distance is lower than the minimum safe distance threshold for two consecutive sampling periods, it is determined to be a collision risk event, including: S331, Based on the main control cycle of the robot control system Maximum refresh cycle of environmental perception sensors The larger of the two values ​​is taken as the system reference period. : ； Set the sampling period for collision detection for integers Times: ； S332, at each sampling time Calculate the current minimum distance Set a length of First-in, first-out queue Store the most recent Distance determination results for each sampling period; queue Each element The value of is determined by the following formula: ； in, Let i be the minimum distance value measured at time i, where i is the index number; When queue All When all elements are 1, it is determined that the value is continuously below the safety threshold, triggering a collision risk event.

5. The collision avoidance method for five-axis robot operation based on the improved RRT algorithm according to claim 1, characterized in that, In step S4, triggering the dynamic replanning mechanism to generate a local optimal path includes the following operations: S41. After a collision risk event is triggered, freeze the current path execution instructions and save a snapshot of the five-axis robot's state at the moment of environmental change. S42. Starting from the state snapshot, the improved RRT algorithm is called within the local space to generate obstacle avoidance sub-paths, including: S421. Read the robot's current pose information and joint velocity data contained in the five-axis robot's state snapshot. Centered on the current position, define a local planning space based on the robot's kinematic limits and a 100-millisecond time constraint. The range of this space is determined by the product of the robot's maximum linear velocity at the end of the robot and the time window. S422. Improved RRT algorithm directional initialization: Set the random tree root node to the current position; Define the local target region: Generate a cone-shaped target sampling region in the local space with the direction of movement as the main axis; Adjusting algorithm parameters: With the goal of satisfying the planning time window constraints, reducing the maximum number of iterations and the expansion step size; S423, Real-time Collision Detection Integration: During the random tree expansion process, for each new node generated, the minimum distance from the node to environmental obstacles is calculated, and the collision risk judgment logic is called. If the minimum distance is consistently lower than the safety threshold, the node is discarded and the expansion is restarted. S424, Path Extraction and Optimization: After reaching the required number of iterations and successfully connecting the target region, extract the local path from the root node to the target point, and use a path smoothing algorithm to smooth the path to ensure continuity with the tangent direction of the global path. S425, Calculation time constraint verification: Monitor the entire process time. If it exceeds 85 milliseconds, trigger the acceleration mechanism: Improve the calculation efficiency by reducing the resolution of the environmental point cloud and shrinking the local planning space, and ensure that the total planning time of the final output path is less than 100 milliseconds. S43. Verify the acceleration continuity at the junction of the sub-path and the global path. If the joint torque abruptly exceeds the limit, replanning is required, including: S431. Extract the joint velocity vectors of the sub-path start point and the global path end point; S432. Calculate the theoretical acceleration value of the path connection point using the finite difference method; S433. When the theoretical acceleration value exceeds the maximum allowable value of the five-axis robot joint servo system, a uniform speed transition section is inserted at the connection point.

6. The collision avoidance method for five-axis robot operation based on the improved RRT algorithm according to claim 1, characterized in that, In step S5, the process of converting the local optimization path into control instructions also includes an exception handling mechanism, which is as follows: S51. When the cumulative number of dynamic replanning failures exceeds the limit, control the five-axis robot to perform an emergency retraction action to the nearest safe pose. S52, trigger the audible and visual alarm device and upload abnormal environmental data to the remote monitoring platform.

7. A five-axis robot collision avoidance system based on an improved RRT algorithm, characterized in that, The method for implementing the five-axis robot collision avoidance method according to any one of claims 1-6 includes: The environment modeling module is configured to: receive environmental data collected by a 3D point cloud scanning device, construct a closed cavity environment model, and set a minimum safe distance threshold; The path planning module includes an improved RRT algorithm unit and a kinematic constraint processing unit. The path planning module is configured to: receive the closed cavity environment model and minimum safe distance threshold output by the environment modeling module; generate an initial collision-free path through the improved RRT algorithm unit; and output path data adapted to the five-axis robot structure through the kinematic constraint processing unit. The real-time monitoring module includes a multi-source sensor acquisition unit and a collision detection algorithm unit. The real-time monitoring module is configured to: receive path data output by the path planning module, acquire robot pose and environmental dynamic data through the multi-source sensor acquisition unit, determine collision risk using the collision detection algorithm unit, and output a risk warning signal. The dynamic response module includes an environment model update unit, a local path replanning unit, and a path smoothing processing unit. The dynamic response module is configured to: receive the risk warning signal output by the real-time monitoring module, refresh the cavity model data through the environment model update unit, generate an optimized path through the local path replanning unit, and output the sub-path after connection verification through the path smoothing processing unit. The execution control module includes a path-command conversion unit and an exception handling unit. The execution control module is configured to: receive sub-path data output by the dynamic response module, generate motor control signals through the path-command conversion unit, drive the five-axis robot joints to perform operations, and output status data, including real-time joint pose, torque load and abnormal alarm information, and manage emergency retraction and alarm processes through the exception handling unit. The system optimization module includes a resource allocation unit and a performance monitoring unit. The system optimization module is configured to: receive environmental dynamic data output by the real-time monitoring module and status data output by the execution control module; calculate the execution efficiency of the optimization algorithm by the resource allocation unit; and have the performance monitoring unit feed back the data to the environment modeling module and the path planning module to adjust the processing parameters.

8. The five-axis robot collision avoidance system based on the improved RRT algorithm according to claim 7, characterized in that, The multi-source sensing acquisition unit is configured as follows: It receives 3D point cloud data of the cavity from 2D lidar, real-time distance data of obstacles from ultrasonic sensors, joint load data from joint torque sensors, contact force data from six-dimensional force sensors, and link temperature data from temperature sensors. It achieves time synchronization of multi-source data through Kalman filtering, eliminates interference signals through the 3σ criterion, and constructs a static and dynamic dual-layer obstacle map by using confidence weighting.

9. The five-axis robot collision avoidance system based on the improved RRT algorithm according to claim 7, characterized in that, The dynamic response module has a built-in replanning priority strategy. This strategy involves activating a real-time trajectory prediction algorithm to optimize the replanning input when a temporary obstacle is detected as a moving object. The implementation of the real-time trajectory prediction algorithm includes the following steps: Dynamic data acquisition: The position sequence and velocity vector of moving obstacles are acquired in real time through a multi-source sensor acquisition unit; Motion state modeling: construct kinematic equations based on the rate of change of obstacle displacement to predict its trajectory within the replanning time window; Trajectory optimization output: The predicted trajectory is mapped to the dynamic obstacle boundary and input into the local path replanning unit of the improved RRT algorithm to generate obstacle avoidance sub-paths.

10. The five-axis robot collision avoidance system based on the improved RRT algorithm according to claim 7, characterized in that, It also includes the deployment of edge computing units, which perform the following collaborative mechanisms: S101. Implement parallel compression processing of point cloud data in the environment modeling module; S102. In the path planning module, GPU acceleration is used to improve the random tree expansion process of the RRT algorithm; S103. Reserve real-time bandwidth for the dynamic response module to ensure that the delay in generating replanning instructions is less than 50ms; S104 provides a computing resource scheduling interface for the system optimization module, dynamically allocating computing power to optimize the overall system performance.

Images