A work protection method and system based on dynamic sweep envelope determination

Abstract

This invention provides a method and system for operational protection based on dynamic sweep envelope determination, relating to the field of data processing technology. The method includes: converting perceived data of the surrounding environment into a set of obstacle spatial points; determining the nearest geometric distance from the obstacle point to the circumference of the smallest circumcircle, using the center position of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, as the safe distance of the obstacle point relative to the subdivided unit; taking the minimum value among all safe distances as the current safe distance; determining the current safety level based on the comparison result between the current safe distance and preset multi-level thresholds; and matching and executing a graded safety interlock strategy corresponding to the safety level to perform corresponding protective actions. This invention improves the safety protection capability and response efficiency of composite robots during dynamic operations.

Description

Technical Field

[0001] This invention relates to the field of data processing technology, and in particular to a method and system for operation protection based on dynamic sweep envelope determination. Background Technology

[0002] In the operation of industrial mobile robotic systems, safety is the fundamental guarantee for system operation. Most existing safety protection methods are designed for static or low-speed motion, relying primarily on preset fixed safety boundaries or simple distance detection to achieve collision avoidance. For example, in traditional AGV (Automated Guided Vehicle) or fixed robotic arm systems, lidar or ultrasonic sensors are typically used to detect obstacles, triggering deceleration or emergency stop when an obstacle enters a preset fixed safety zone. These methods are well-suited for scenarios where the chassis moves independently or the robotic arm operates independently. However, the situation is different when a robotic system performs mobile operations. For instance, when a robotic system needs to adjust the robotic arm's posture synchronously during chassis movement to prepare for grasping or placing operations, the robotic arm's end effector and its links will generate dynamically changing sweeping space with joint movement. Existing fixed safety boundary methods struggle to describe this dynamically changing sweeping volume, potentially failing to provide a sufficiently fitting protective boundary description for a moving robotic arm. This leads to situations where the triggering timing of safety protection strategies is inappropriate in practical applications, potentially affecting the continuity of operations or increasing the risk of collisions.

[0003] Furthermore, in existing methods, the safety area of ​​the chassis and the motion envelope of the robotic arm are usually treated as independent objects and are not treated as separate objects. There is a lack of a unified dynamic fusion description method between the two. When the chassis and the robotic arm move simultaneously, the system has some difficulty in determining the real-time safety projection range of the entire equipment in the direction of travel, which affects the actual effect of safety protection. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a method and system for operation protection based on dynamic sweep envelope determination, which improves the safety protection capability and response efficiency of composite robots in dynamic operation processes.

[0005] To solve the above-mentioned technical problems, the technical solution of the present invention is as follows: Firstly, a job protection method based on dynamic sweep envelope determination, the method comprising: Step 1: Real-time acquisition of motion state data of the composite robot's mobile chassis, joint state and pose data of the robotic arm, end-effector load state data, and perception data of the surrounding environment. Step 2: Based on the joint state and pose data of the robotic arm and the end-effector load state data, obtain the three-dimensional motion sweep volume envelope within a preset time window; based on the motion state data of the mobile chassis and the preset chassis body dimensions, obtain the chassis dynamic safety envelope; project the three-dimensional motion sweep volume envelope and the chassis dynamic safety envelope onto the chassis driving reference plane and fuse them to obtain a two-dimensional composite safety boundary profile; subdivide the two-dimensional composite safety boundary profile to obtain multiple subdivided units; calculate the minimum circumcircle of each subdivided unit, and the overall dynamic safety boundary description is composed of all minimum circumcircles. Step 3: Convert the perceived data of the surrounding environment into a set of obstacle spatial points. Using the center of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, determine the shortest geometric distance from the obstacle point to the circumference of the smallest circumcircle, which is used as the safe distance of the obstacle point relative to the subdivision unit. Take the minimum value among all safe distances as the current safe distance. Determine the current safety level based on the comparison result between the current safe distance and the preset multi-level threshold. Step 4: Based on the security level, match and execute the graded security interlock strategy corresponding to the security level to perform the corresponding protective actions.

[0006] Secondly, an operational protection system based on dynamic sweep envelope determination includes: The multi-source data acquisition module is used to acquire in real time motion state data of the composite robot's mobile chassis, joint state and pose data of the robotic arm, end-effector load state data, and perception data of the surrounding environment. The dynamic envelope modeling module is used to obtain the 3D motion sweep volume envelope within a preset time window based on the joint state and pose data of the robotic arm and the end-effector load state data; to obtain the chassis dynamic safety envelope based on the motion state data of the mobile chassis and the preset chassis body dimensions; to project and fuse the 3D motion sweep volume envelope and the chassis dynamic safety envelope onto the chassis driving reference plane to obtain a 2D composite safety boundary profile; to subdivide the 2D composite safety boundary profile to obtain multiple subdivided elements; to calculate the minimum circumcircle of each subdivided element, and to form the overall dynamic safety boundary description by all minimum circumcircles. The safety distance determination module converts the perceived data of the surrounding environment into a set of obstacle spatial points. Using the center of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, it determines the nearest geometric distance from the obstacle point to the circumference of the smallest circumcircle, which is taken as the safety distance of the obstacle point relative to the subdivision unit. The minimum value among all safety distances is taken as the current safety distance. Based on the comparison between the current safety distance and the preset multi-level thresholds, the current safety level is determined. The execution module is used to match and execute the hierarchical security interlock strategy corresponding to the security level, so as to perform the corresponding protective actions.

[0007] Thirdly, a computing device, comprising: One or more processors; A storage device for storing one or more programs that, when executed by one or more processors, cause the one or more processors to implement the method.

[0008] Fourthly, a computer-readable storage medium storing a program that, when executed by a processor, implements the method.

[0009] The above-described solution of the present invention has at least the following beneficial effects: This invention can acquire real-time data on chassis motion, robotic arm joint pose, and end-effector load conditions. Combined with a preset time window, it predicts the robotic arm's 3D sweep volume envelope and the chassis's dynamic safety envelope. This allows for real-time updates to the safety protection range based on the robot's operational status, better matching the actual space occupancy during dynamic operations and improving the fit and effectiveness of the safety boundary under dynamic conditions. By identifying concave points on the composite boundary contour, it performs convex subdivision, breaking down the irregular overall contour into several non-overlapping convex subdivision units. The overall dynamic safety boundary is then constructed using the minimum circumcircle of each unit. This layered processing method effectively simplifies the geometric calculation difficulty of complex boundaries and improves computational efficiency. Simultaneously, based on the geometric distance calculation logic between the minimum circumcircle and obstacle points, it can effectively obtain the shortest safe distance between obstacles and the robot's protection boundary, improving the reliability of risk assessment and better meeting the dual requirements of detection effect and response speed for high-frequency dynamic operations of industrial robots. Attached Figure Description

[0010] Figure 1 This is a schematic flowchart of an operation protection method based on dynamic sweep envelope determination provided by an embodiment of the present invention.

[0011] Figure 2 This is a schematic diagram of an operation protection system based on dynamic sweep envelope determination provided by an embodiment of the present invention. Detailed Implementation

[0012] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.

[0013] like Figure 1 As shown, an embodiment of the present invention proposes an operation protection method based on dynamic sweep envelope determination, the method comprising the following steps: Step 1: Real-time acquisition of motion state data of the composite robot's mobile chassis, joint state and pose data of the robotic arm, end-effector load state data, and perception data of the surrounding environment. Step 2: Based on the joint state and pose data of the robotic arm and the end-effector load state data, obtain the three-dimensional motion sweep volume envelope within a preset time window; based on the motion state data of the mobile chassis and the preset chassis body dimensions, obtain the chassis dynamic safety envelope; project the three-dimensional motion sweep volume envelope and the chassis dynamic safety envelope onto the chassis driving reference plane and fuse them to obtain a two-dimensional composite safety boundary profile; subdivide the two-dimensional composite safety boundary profile to obtain multiple subdivided units; calculate the minimum circumcircle of each subdivided unit, and the overall dynamic safety boundary description is composed of all minimum circumcircles. Step 3: Convert the perceived data of the surrounding environment into a set of obstacle spatial points. Using the center of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, determine the shortest geometric distance from the obstacle point to the circumference of the smallest circumcircle, which is used as the safe distance of the obstacle point relative to the subdivision unit. Take the minimum value among all safe distances as the current safe distance. Determine the current safety level based on the comparison result between the current safe distance and the preset multi-level threshold. Step 4: Based on the security level, match and execute the graded security interlock strategy corresponding to the security level to perform the corresponding protective actions.

[0014] In this embodiment of the invention, the invention can acquire real-time data on chassis motion status, robotic arm joint pose, and end-effector load conditions. Combined with a preset time window, it predicts the robotic arm's three-dimensional sweep volume envelope and the chassis's dynamic safety envelope. This allows for real-time updates to the safety protection range based on the robot's operational status, better matching the actual space occupancy during dynamic operations and improving the fit and effectiveness of the safety boundary under dynamic conditions. By identifying concave points on the composite boundary contour, convex subdivision is completed, splitting the irregular overall contour into several non-overlapping convex subdivision units. The overall dynamic safety boundary is then constructed using the minimum circumcircle of each unit. This layered processing method effectively simplifies the geometric calculation difficulty of complex boundaries and improves computational efficiency. Simultaneously, based on the geometric distance calculation logic between the minimum circumcircle and obstacle points, the shortest safe distance between the obstacle and the robot's protection boundary can be effectively obtained, improving the reliability of risk assessment and better meeting the dual requirements of detection effect and response speed for high-frequency dynamic operations of industrial robots.

[0015] In a preferred embodiment of the present invention, the execution process of step 1 is as follows: The motion status data of the mobile chassis includes four items: linear velocity, angular velocity, heading angle, and position coordinates. The linear velocity is obtained as follows: read the pulse increment of the chassis drive wheel encoder per unit time; divide the pulse increment by the number of pulses per revolution to obtain the number of revolutions of the drive wheel per unit time; multiply the number of revolutions by the outer circumference of the drive wheel to obtain the forward distance of the drive wheel; divide the forward distance by the unit time to obtain the encoder-estimated linear velocity; simultaneously, read the linear acceleration value from the inertial measurement unit, integrate the linear acceleration value over time, and the integration result is the inertial-estimated linear velocity; fuse the encoder-estimated linear velocity and the inertial-estimated linear velocity through complementary filtering. The complementary filtering rule is: multiply the encoder-estimated value by a preset high-pass filter weight coefficient, multiply the inertial-estimated value by a preset low-pass filter weight coefficient, and the sum of the high-pass weight coefficient and the low-pass weight coefficient is 1. The resulting weighted sum is the fused linear velocity output value. Combining the operating characteristics of the composite robot chassis speed measurement with the sensor performance, two types of weighting coefficients are set with fixed value ranges. The high-pass filter weighting coefficient ranges from 0.90 to 0.99, and the low-pass filter weighting coefficient ranges from 0.01 to 0.10, strictly meeting the requirement that the high-pass weighting coefficient is greater than the low-pass weighting coefficient. Under steady-state uniform speed driving conditions, the high-pass weighting coefficient at the upper limit of the range and the low-pass weighting coefficient at the corresponding lower limit of the range are selected first. This can suppress the slow cumulative drift caused by the integration of the inertial measurement unit to the greatest extent and ensure the long-term stability of speed measurement. Under transient change conditions such as robot start-stop and speed change, the high-pass weighting coefficient can be appropriately lowered and the low-pass weighting coefficient can be correspondingly increased. Within the compliant value range, this can improve the confidence of the inertial measurement data, make up for the defect of the encoder speed measurement dynamic response lag, and balance the speed measurement accuracy and dynamic adaptability.

[0016] The angular velocity is obtained by directly reading the angular velocity measurement value around the vertical axis from the gyroscope of the inertial measurement unit; simultaneously, the difference between the linear velocities of the left and right drive wheels is divided by the wheelbase to obtain the kinematic solution value of the angular velocity; the gyroscope measurement value and the kinematic solution value are weighted and averaged to obtain the final output angular velocity, with the gyroscope measurement value having a higher weight. The heading angle is obtained by initially calibrating the heading angle at system startup using the geometric relationship between the chassis and known reference points on the site; during operation, the vertical axis angular velocity value output by the gyroscope is integrated over time, with the integral increment for each cycle equal to the angular velocity value of the current cycle multiplied by the control cycle duration, and the integral increment is added to the heading angle of the previous cycle. The estimated heading angle is obtained. In each preset correction cycle, the currently scanned point cloud of the lidar is scanned and matched with a pre-built site grid map. The search searches for the rotation angle that optimizes the matching degree between each laser reflection point in the point cloud and the grid cell occupied by the map. The optimal rotation angle is used as the absolute heading angle observation value. The search process is as follows: within a preset angle search window near the estimated heading angle, candidate rotation angles are tried one by one with a preset angle step size. For each candidate rotation angle, the current point cloud is rotated around the vertical axis by that angle and then projected onto the grid map. The number of points in the point cloud falling into the occupied grid cell of the map is counted as the matching score. The candidate rotation angle with the highest matching score is taken as the absolute heading angle observation value. The difference between the estimated heading angle and the absolute heading angle observation value is calculated to obtain the heading angle correction amount. This correction amount is added to the estimated heading angle to eliminate cumulative drift before output.

[0017] The position coordinates are obtained by decomposing the linear velocity along the heading angle into two components: the horizontal axis velocity component is equal to the linear velocity multiplied by the cosine of the heading angle, and the vertical axis velocity component is equal to the linear velocity multiplied by the sine of the heading angle. The two components are multiplied by the control cycle duration to obtain the horizontal and vertical displacement increments for the current cycle, which are then added to the position coordinates of the previous cycle to obtain the dead reckoning position. In each preset correction cycle, the absolute position observation values ​​are obtained simultaneously using lidar scanning and matching, i.e., the optimal horizontal and vertical offsets obtained during the translation search during matching. The difference between the absolute position observation values ​​and the dead reckoning position is added as a correction to the dead reckoning position and then output.

[0018] Acquisition of robotic arm joint states and pose data: The robotic arm data includes four items: joint angle, joint angular velocity, joint angular acceleration, and end-effector pose.

[0019] The joint angle is obtained by reading the current count value from the encoder installed at each joint, subtracting the zero offset value determined by the calibration process after the robot arm is assembled from the count value to obtain the effective count value, and multiplying the effective count value by the encoder's angular resolution, i.e., the angle value corresponding to each counting pulse, to obtain the joint angle value; the encoder's angular resolution is equal to 360 degrees divided by the total count per encoder revolution.

[0020] The joint angular velocity is obtained by taking the difference between the joint angles of the current cycle and the previous cycle, and dividing the difference by the control cycle duration. The joint angular acceleration is obtained by taking the difference between the joint angular velocity of the current cycle and the previous cycle, and dividing the difference by the control cycle duration.

[0021] Obtaining the end-effector pose requires executing a complete forward kinematics calculation chain. The following details all intermediate steps of this calculation chain from joint angle input to end-effector pose output: First, a local coordinate system is established for each joint of the robotic arm, fixed to that joint. The establishment of the coordinate system follows these conventions: For the i-th joint, the vertical axis of its coordinate system coincides with the rotation or translation axis of that joint, and the direction can be arbitrarily chosen, but all joints must maintain consistency; the horizontal axis of its coordinate system points to the next joint along the common perpendicular line between that joint and the next joint; the vertical axis of its coordinate system is determined by the right-hand rule of the vertical and horizontal axes; using the base coordinate system as a reference, four geometric parameters are defined for each joint from the base towards the end effector to describe... Describe the spatial relationship between adjacent joint coordinate systems; these four parameters are: link length, link torsion angle, joint offset, and joint angle; link length refers to the length of the common perpendicular between the vertical axes of two adjacent joints, that is, the distance between the horizontal axes of the preceding and following joints along this common perpendicular; link torsion angle refers to the angle required for the vertical axis of the following joint to rotate around this common perpendicular from the direction of the vertical axis of the preceding joint to the direction of the vertical axis of the following joint; joint offset refers to the distance along its vertical axis in the coordinate system of the preceding joint from the horizontal axis of the preceding joint to the horizontal axis of the following joint. Joint angle refers to the angle required for the horizontal axis of the following joint to rotate around the vertical axis of the preceding joint to the direction of the horizontal axis of the preceding joint; for rotary joints, the joint angle is a variable, and the other three parameters are constants; for translational joints, the joint offset is a variable, and the other three parameters are constants.

[0022] Using the four parameters mentioned above, a spatial transformation relationship is constructed for each pair of adjacent joints, transforming points in the coordinate system of the latter joint to the coordinate system of the former joint. This transformation consists of two rotation operations and two translation operations combined sequentially. The overall transformation effect is as follows: First, rotate the coordinate system of the latter joint around its own vertical axis by a joint angle value, so that the horizontal axis of the latter joint is collinear with that of the former joint; then translate the joint offset by a distance along the rotated vertical axis, so that the origins of the two coordinate systems are aligned in the vertical direction; then translate the length of the link along the direction of the common perpendicular, i.e., the horizontal axis, so that the origins of the two coordinate systems completely coincide; finally, rotate the torsion angle of the link around the common perpendicular, i.e., the horizontal axis, so that the vertical axes of the two coordinate systems are aligned.

[0023] For a robotic arm with multiple joints, the transformation relationships described above for each joint are continuously superimposed in order from the base to the end effector. The superposition rule is as follows: multiply the transformation matrix from the base coordinate system to the first joint coordinate system with the transformation matrix from the first joint coordinate system to the second joint coordinate system, multiply the result by the transformation matrix from the second joint to the third joint, and so on, until the transformation matrix from the last joint to the end effector flange coordinate system is included. The final multiplication result is the transformation relationship from the base coordinate system to the end effector flange coordinate system. This transformation relationship contains all the information about the end effector pose: the three elements in the 1st row and 4th column, the 2nd row and 4th column, and the 3rd row and 4th column of the final transformation matrix are taken as the horizontal, vertical, and axial coordinates of the end effector reference point in the base coordinate system, respectively; the top-left 3x3 submatrix of the final transformation matrix represents the direction vectors of the three coordinate axes of the end effector coordinate system in the base coordinate system, i.e., the pose of the end effector.

[0024] The above calculation is performed once in each control cycle, taking the joint angle values ​​after zero-position offset correction of the encoder readings of each joint as input, to obtain the real-time output of the end pose.

[0025] Acquisition of end-load status data and construction of workpiece parameter database: The end-effector load status data includes five items: load presence identifier, workpiece type, workpiece geometry, workpiece mass, and installation offset. The load presence identifier is obtained by reading the closing position signal of the end-effector gripper's jaws and the trigger signal of the in-situ detection sensor. If both signals are valid, the identifier is set to the gripped state; otherwise, it is set to the ungripped state. The workpiece type is obtained by reading the workpiece model code specified in the current work order from the job task management system. This code is then used to perform a primary key search in the workpiece parameter database to return the corresponding record. The workpiece mass is obtained by reading the force component measurement value in the vertical direction from the six-dimensional force sensor at the end-effector wrist after the robotic arm stably grips the workpiece. This force component is then divided by the local gravitational acceleration to obtain the workpiece mass. The installation offset is obtained by retrieving the values ​​of the horizontal axis offset, vertical axis offset, and vertical axis offset fields corresponding to the workpiece type from the workpiece parameter database.

[0026] The following details the construction process and internal structure of the workpiece parameter database: The workpiece parameter database contains two main tables and one index table. The first table is the workpiece basic information table. Each record in this table corresponds to a workpiece type and includes the following fields: Workpiece type code field, which serves as the unique primary key of this table and uses a combination of numbers and letters for encoding, uniformly assigned by the material management system; Workpiece name field, which describes the general name of the workpiece in Chinese; Workpiece category field, which distinguishes between three categories: cuboid workpieces, cylindrical workpieces, and irregularly shaped workpieces; Workpiece length field, workpiece width field, and workpiece height field, which store the actual external dimensions in three directions for cuboid workpieces, the outer diameter in the length field, the outer diameter in the width field, and the length in the height field for cylindrical workpieces, and the three dimensions of its outer enclosure for irregularly shaped workpieces; Workpiece nominal quality field, which stores the nominal mass value of this type of workpiece for verification and comparison with force sensor measurements; Workpiece material field, which stores the material grade of the workpiece.

[0027] The second table is the clamping offset table. Each record in this table corresponds to a combination of a workpiece type and an end-feed fixture type, and includes the following fields: a combined code field, which is a joint primary key composed of the workpiece type code and the fixture type code; a horizontal offset field, which stores the offset distance of the workpiece geometric center relative to the end flange center in the horizontal direction; a vertical offset field, which stores the offset distance in the vertical direction; and a vertical offset field, which stores the offset distance in the vertical direction. Positive and negative values ​​of the offset amount indicate the offset direction.

[0028] The third table is the fixture type index table, which contains the following fields: fixture type code field, primary key; fixture name field; fixture reference point offset field, which stores the fixed offset between the fixture reference point and the center of the end flange in the three directions of horizontal axis, vertical axis, and vertical axis.

[0029] The initial construction process of the workpiece parameter database is as follows: During the system deployment phase, process engineers input the basic information of all workpiece types on the production line; mechanical engineers input the reference point offsets of all fixture types; and system integration personnel input the offset amount for each workpiece and fixture combination. This offset amount is obtained through actual measurement in the calibration process. The calibration process involves using the end flange of the robotic arm as a reference, touching the calibration reference point at a known position, recording the end pose reading at this time, and comparing the pose with the known pose of the calibration reference point to obtain the offset amount. After the initial database construction is completed, it is exported as a structured file and deployed to the control system storage of the composite robot. If a new workpiece type is added during operation, the above input process is executed for the new record and the database is re-exported and deployed. The retrieval interface of the workpiece parameter database accepts a workpiece type code as a query keyword, locates the corresponding record in the workpiece basic information table, and simultaneously retrieves the offset amount in the clamping offset table using the combination of the workpiece type code and the currently installed fixture type code as the keyword. The retrieval results of the two tables are merged into a complete data structure and returned.

[0030] Acquisition of surrounding environment perception data: The surrounding environment perception data includes three categories: the spatial location of obstacles, the movement information of dynamic obstacles, and the location of fixed facilities.

[0031] The spatial location of obstacles is obtained as follows: For each reflection point of the 2D LiDAR, the distance value carried by that point is multiplied by the cosine of the scanning angle to obtain the local coordinates on the horizontal axis with the LiDAR as the origin, and the distance value is multiplied by the sine of the scanning angle to obtain the local coordinates on the vertical axis. Then, the local coordinates are converted into 3D coordinates in the world coordinate system through the LiDAR extrinsic parameter transformation matrix, where the height coordinate is taken as the installation height of the LiDAR. For the point cloud of the 3D LiDAR, it is directly converted into world coordinates point by point through its extrinsic parameter transformation matrix. For each effective pixel of the depth camera, the operation steps of backprojection using camera intrinsic parameters are as follows: take the difference between the pixel's horizontal axis image coordinate and the principal point's horizontal axis coordinate, divide by the horizontal axis focal length to obtain the normalized horizontal axis coordinate; take the difference between the pixel's vertical axis image coordinate and the principal point's vertical axis coordinate, divide by the vertical axis focal length to obtain the normalized vertical axis coordinate; multiply the normalized horizontal axis coordinate and the normalized vertical axis coordinate by the depth value corresponding to the pixel to obtain the horizontal axis position and vertical axis position in the camera coordinate system, respectively. The depth value is directly used as the height position in the camera coordinate system, and then converted to world coordinates through the camera extrinsic parameter transformation matrix; for the ranging value of the ultrasonic sensor, take the sensor installation position as the starting point, extend the measured distance along the sensor's detection centerline to obtain the spatial coordinates of the obstacle. After aggregating all the spatial points from the above sources, a triple filtering process is performed: self-occlusion filtering removes points located within the volume currently occupied by the composite robot; distance filtering removes points whose distance to the composite robot's reference point exceeds a preset maximum sensing distance, which is the smaller of the farthest effective range of all sensors and the safety judgment focus range, ranging from several meters to tens of meters; height filtering retains only points whose height coordinates are above the ground and do not exceed the maximum extension height of the robotic arm plus a preset safety margin; if the point cloud density is too high, downsampling is performed according to a preset voxel side length, retaining only the point closest to the center of each voxel grid. The final set of retained points is the obstacle spatial point set.

[0032] The method for acquiring dynamic obstacle motion information is as follows: target association tracking is performed between obstacle spatial point sets in multiple consecutive frames, and the magnitude and direction of its motion speed are obtained by dividing the change in spatial position of the same physical obstacle between frames by the time interval between frames; the location information of fixed facilities is loaded from a pre-built static site occupation grid map, in which the outline boundaries of structures such as walls, columns, and equipment are marked in advance in the form of occupation grids.

[0033] In a preferred embodiment of the present invention, step 2 is performed as follows: The preset time window length is determined by the following rules: The base duration is an integer multiple of the system control cycle length. The base duration is then checked to ensure it meets the coverage condition: This condition is determined by whether the spatial displacement of the robotic arm's end effector within this base duration, while the robot is at its designed maximum speed, is greater than or equal to the total displacement caused by inertia during the period from when the obstacle is detected by the sensor, through system processing and communication delays, until the highest level of protection action is completed. This total displacement is removed, and the minimum required window duration is obtained by calculating the maximum linear velocity of the robotic arm's end effector. If the base duration is less than the minimum required window duration, the base duration is extended cycle by cycle until it is greater than or equal to the minimum required window duration. For applications with higher safety standards, a safety margin factor greater than 1 is multiplied by the coverage condition. The higher the safety standard, the larger this factor becomes, ranging from 1 to 3. Motion prediction of the robotic arm is then performed, with the input being the current angle and angular velocity values ​​of each joint, as well as the end effector load status data. The process is then tailored to the specific motion control conditions currently applied to the robotic arm.

[0034] The robotic arm is executing a planned motion command, which includes target angle values ​​for each joint and a total motion duration. First, the ratio of the executed time to the total motion duration is calculated to obtain the motion completion percentage. The speed change during the motion follows a preset speed curve, constructed as follows: the total motion duration is divided into three phases: the acceleration phase duration equals the total motion duration multiplied by a preset acceleration time percentage; the constant speed phase duration equals the total motion duration multiplied by a preset constant speed time percentage; and the deceleration phase duration equals the total motion duration minus the acceleration and deceleration phase durations. Within the acceleration phase, the joint angular velocity... The angular velocity increases linearly from zero with a constant angular acceleration. The absolute value of the angular acceleration is equal to the maximum angular velocity divided by the duration of the acceleration phase. The direction is towards the target angle. The angular velocity reaches its maximum at the end of the acceleration phase. During the uniform motion phase, the angular velocity remains constant at the maximum angular velocity. During the deceleration phase, the angular velocity decreases linearly from the maximum angular velocity to zero with a constant angular deceleration. The absolute value of the angular deceleration is equal to the maximum angular velocity divided by the duration of the deceleration phase. The maximum angular velocity is equal to the total target angular displacement divided by the ratio of the total motion duration to half of the acceleration time, half of the uniform motion time, and half of the deceleration time.

[0035] For the position on the velocity curve corresponding to the current moment of motion completion percentage, determine whether the current moment is in the acceleration phase, constant speed phase, or deceleration phase: if the current execution duration is less than or equal to the acceleration phase duration, it is in the acceleration phase; if the current execution duration is greater than the acceleration phase duration but less than the sum of the acceleration phase duration and the constant speed phase duration, it is in the constant speed phase; if the current execution duration is greater than or equal to the sum of the acceleration phase duration and the constant speed phase duration, it is in the deceleration phase. The current displacement completion percentage is obtained by removing the angle already covered on the velocity curve at the current moment and calculating the target total angular displacement. Based on the current displacement completion percentage, add the displacement completion percentage increment corresponding to the preset time window to obtain the displacement completion percentage at the end of the window; the calculation of the window displacement completion percentage increment... The calculation method is as follows: If the current moment is in the acceleration phase, the increment is equal to the sum of the current angular velocity multiplied by the window duration and 1 / 2 multiplied by the square of the angular acceleration multiplied by the window duration, then divided by the target total angular displacement; if the current moment is in the uniform velocity phase, the increment is equal to the maximum angular velocity multiplied by the window duration divided by the target total angular displacement; if the current moment is in the deceleration phase, the increment is equal to the difference between the current angular velocity multiplied by the window duration and 1 / 2 multiplied by the square of the absolute value of the angular deceleration multiplied by the window duration, then divided by the target total angular displacement; the minimum value of the sum of the current displacement completion ratio and the window displacement completion ratio increment is taken to obtain the displacement completion ratio at the end of the window; the displacement completion ratio at the end of the window is multiplied by the target total angular displacement to obtain the expected angle value of each joint at the end of the window.

[0036] The robotic arm is currently in a free-dragging state and has not executed any motion commands. At this time, the expected angle at the end of the window is calculated based on the condition that the current angular velocity remains constant: the expected angle value is obtained by adding the current angle value to the product of the current angular velocity and the preset time window duration. For end load consideration: when the load is marked as grasped, the distance from the center of the end flange to the farthest point of the workpiece is calculated as the equivalent extension length based on the workpiece's geometry and installation offset. This equivalent extension length replaces the original end link length value in the subsequent forward kinematics calculation to ensure that the sweep coverage includes the entire outline of the workpiece.

[0037] After determining the current joint angle and the expected joint angle, discrete sampling with equal time intervals is performed within the angular range between the two. The sampling interval is determined by using the maximum possible linear velocity in space of the farthest end of each link of the robotic arm as the constraint benchmark. The maximum possible linear velocity is calculated as follows: for each joint, the absolute value of the current angular velocity of the joint is taken and multiplied by the transmission radius from the joint axis to the farthest end, which is equal to the sum of the projected distances of all link lengths from the joint axis to the farthest end in the direction perpendicular to the joint axis. The maximum value among the calculation results for all joints is the maximum possible linear velocity. The preset displacement accuracy constraint value is divided by the maximum possible linear velocity to obtain the maximum allowable sampling time interval. The displacement accuracy constraint value is set according to the spatial resolution required for safety judgment, and is on the order of several millimeters to tens of millimeters. The final sampling time interval is an integer value that does not exceed the maximum allowable value.

[0038] For each sampling moment obtained after discretization, based on the joint angle values ​​corresponding to that sampling moment, forward kinematics is performed to calculate the position and orientation of the equivalent geometry of each link and the equivalent geometry of the end load in three-dimensional space. The equivalent geometry of each link is selected according to the actual shape of the link: for slender cylindrical links, a cylinder is constructed with the line connecting the centers of the two end sections as the axis and the link radius as the radial radius; for non-cylindrical box-type links, its circumscribed cuboid is used as the equivalent geometry; the equivalent geometry of the end load is constructed with the workpiece geometry as the reference to construct a circumscribed cuboid, and increases in length, width, and height. A preset safety margin value, combined with the installation offset and the end pose at the sampling moment, determines the position and orientation of the cuboid in space. The safety margin value is fixed in a specific range of 5 mm to 50 mm, which can be adjusted according to the workpiece size and operation accuracy requirements. For small precision workpieces, a value in the range of 5 mm to 20 mm is selected, while for medium and large-sized conventional workpieces, a value in the range of 20 mm to 50 mm is selected. This can accurately cover minor installation deviations and pose calculation errors of the workpiece shape, while avoiding the problems of excessive safety protection range due to excessively large margin settings and blind spots due to excessively small margin settings. A spatial union operation is performed on the space occupied by all equivalent geometric bodies at all sampling moments. The operation rule is that any point in three-dimensional space is included in the union result if it is occupied by any equivalent geometric body at any sampling moment. The three-dimensional spatial region obtained by summing all spatial points that satisfy this rule is the three-dimensional motion sweep volume envelope within the preset time window.

[0039] The inputs are the chassis's real-time linear velocity, real-time angular velocity, and current heading angle. Straight-line driving and turning are distinguished based on whether the absolute value of the chassis angular velocity is less than a preset threshold. The threshold is determined by the noise level of the angular velocity sensor, typically 2 to 3 times the root mean square (RMS) of the sensor noise. When the absolute value of the angular velocity is less than the threshold, the chassis is approximately driving in a straight line; the trajectory is a straight line segment extending from the chassis's current position along the heading angle. The starting point of the straight line segment is the chassis's current position, the direction is determined by the heading angle, and the length of the straight line segment is the product of the current linear velocity and the duration of a preset time window. When the absolute value of the angular velocity is greater than or equal to the threshold, the chassis is turning; the trajectory is an arc. The construction process of the arc... The following steps are used to determine the arc's radius: The radius of the arc is the quotient of the absolute value of the linear velocity divided by the absolute value of the angular velocity. The center of the arc is determined as follows: Draw an auxiliary straight line perpendicular to the heading angle through the current chassis position. The direction angle of this auxiliary line is equal to the heading angle plus or minus 90 degrees. The specific addition or subtraction depends on the sign of the angular velocity: a positive angular velocity (counter-clockwise turn) is added by 90 degrees, and a negative angular velocity (clockwise turn) is subtracted by 90 degrees. Measure a radius outward from the current position along this auxiliary straight line to obtain the coordinates of the arc's center. The starting point of the arc is the current chassis position. The central angle of the arc's travel is equal to the product of the absolute value of the angular velocity and the duration of a preset time window. The ending point of the arc is the position reached after traveling the arc length corresponding to this central angle from the starting point. The arc length is equal to the product of the central angle and the radius.

[0040] The chassis body contour is pre-stored in the system as a sequence of closed polygon vertices. This polygon is the outer contour boundary obtained by projecting all fixed components of the chassis onto a horizontal plane. This polygon is continuously moved from the starting position to the ending position along the aforementioned driving trajectory. The union of all planar areas swept and occupied by the polygon during the movement is the chassis dynamic safety envelope. For the case of a straight trajectory, the specific construction method of the chassis dynamic safety envelope is as follows: the posture of the chassis projection polygon at the starting point of the trajectory is taken as the starting end face; the posture of the chassis projection polygon at the ending point of the trajectory after translating it from the starting point along the trajectory direction by one trajectory length is taken as the ending end face; each vertex of the chassis projection polygon is taken from the starting point... The connecting line segment formed by translating the point position along the trajectory direction to the endpoint position is taken as the trajectory line segment of that vertex; the closed area enclosed by connecting the starting end face, the ending end face, and the part of the trajectory line segment located outside the envelope is the chassis dynamic safety envelope; for the case of circular arc trajectory, the above-mentioned translation is not a straight line translation but a circular arc rotation; the chassis projection polygon rotates around the center of the arc, from the initial rotation angle of the starting point to the target rotation angle of the endpoint. During the rotation, the area swept by the polygon is a circular arc strip area, whose outer circular arc boundary is generated by the outer vertex of the polygon along the maximum rotation radius, and the inner circular arc boundary is generated by the inner vertex of the polygon along the minimum rotation radius.

[0041] The observation direction of the projection is perpendicular to the chassis driving reference plane, i.e., vertically downward or vertically upward, both of which are equivalent in the projection result. The three-dimensional motion sweep volume envelope is regarded as an opaque entity along the observation direction. A parallel beam of light is shone from the observation direction, casting a shadow on the chassis driving reference plane. The specific operation process for extracting the outer contour boundary of this shadow is as follows: traverse all points on the surface of the three-dimensional motion sweep volume envelope; for each point on the surface, draw a projection line from that point along the observation direction toward the chassis driving reference plane, and check the direction from that point toward the reference plane on the projection line. Are there any other points belonging to the 3D envelope? If not, then the point belongs to the silhouette outline point. Extract the horizontal and vertical coordinates of all the selected silhouette outline points, discard the height coordinates, and obtain a set of 2D mapping points. Determine the outer boundary of this set of 2D mapping points. The operation to determine the boundary is as follows: find the point with the smallest horizontal coordinate among all the 2D mapping points as the starting point. Using the starting point as the reference, calculate the polar angle of each of the remaining points to the starting point, that is, the angle between the line connecting the point and the starting point and the positive direction of the horizontal axis. The angle is obtained by dividing the vertical component of the line by the arctangent value of the horizontal component. Sort all points in ascending order of polar angle; if multiple points have equal polar angles, retain only the point furthest from the starting point; starting from the starting point, connect adjacent points sequentially in sorted order to form an initial polygonal path; starting from the third point on this polygonal path, perform an inward folding test on each consecutive three-point combination on the path: label these three points sequentially as the front point, middle point, and back point; take the vector from the front point to the middle point as the first vector; take the vector from the middle point to the back point as the second vector; calculate the product of the horizontal component of the first vector and the vertical component of the second vector, minus the product of the horizontal component of the first vector and the vertical component of the second vector. The cross product of the horizontal components of the second vector is used to determine the midpoint. If the cross product is negative, the midpoint is considered an inward fold point. The midpoint is then removed from the path, and the previous three-point combination is checked back until a broken line on the midpoint no longer folds inward. If the cross product is positive, the midpoint is considered an outward fold point and is retained. After all checks are completed, the closed broken line formed by connecting the end of the remaining vertex sequence is the boundary of a convex polygon with all interior angles not exceeding 180 degrees. The internal region enclosed by this boundary is the first projection region. This first projection region completely covers all two-dimensional mapping points, and its area is the smallest among all convex polygons that satisfy this condition.

[0042] Perform a polygon union operation on the first and second projection regions to obtain a two-dimensional composite safety boundary profile. The rules for the union operation are as follows: input the boundary vertex sequence of the first and second projection regions; find all intersection points between the two boundaries by performing cross-checking on each pair of edges. The cross-checking method is to combine each edge of the first boundary with each edge of the second boundary in pairs, and check whether each pair of edge segments intersects at a non-endpoint. If they intersect, record the coordinates of the intersection point and split the two edges at the intersection point; break the edges of the two boundary polygons at all intersection points and reassemble them into several closed loops; take the outermost loop with the largest area among all closed loops as the outer boundary of the union, and the continuous planar region enclosed by this outer boundary is the two-dimensional composite safety boundary profile.

[0043] Along the closed boundary curve of the two-dimensional composite safety boundary profile, equal arc length sampling is performed at a preset arc length sampling interval to obtain a series of ordered boundary points. The sampling interval is determined by dividing the total perimeter of the profile by a preset sampling resolution parameter. This parameter is set such that the straight-line distance between adjacent sampling points does not exceed one-tenth of the total perimeter, ensuring sufficient sampling accuracy to characterize the geometric features of the profile. The sampling points are logically closed at both ends, forming a closed boundary point sequence. Each point stores its two-dimensional coordinates on the chassis driving reference plane. Each boundary point in the sequence is examined sequentially along the surrounding direction of the closed boundary point sequence to determine whether it is a concave point. For the currently examined boundary point, its previous and next neighbor points are taken, with the current point as the common vertex. The first line segment from the current point to the previous neighbor point and the second line segment from the current point to the next neighbor point are constructed. The angle between these two line segments on one side inside the profile, i.e., the interior angle, is calculated.

[0044] The calculation of interior angles is performed in three steps: To calculate the minimum angle between two direction vectors, first obtain the direction vector from the previous neighboring point to the current point and the direction vector from the current point to the next neighboring point; sum the products of the horizontal and vertical components of the two direction vectors to get the dot product; sum the squares of the two components of each direction vector and then take the square root to get the length of the vector; divide the dot product by the product of the lengths of the two vectors to get the cosine of the angle; take the inverse cosine of the cosine of the angle to get the minimum angle between the two vectors, with a range of zero to 180 degrees.

[0045] To calculate the rotation direction, multiply the x-axis component of the vector pointing from the previous neighbor to the current point by the y-axis component of the vector pointing from the current point to the next neighbor, and subtract the product of the y-axis component of the vector pointing from the previous neighbor to the current point and the x-axis component of the vector pointing from the current point to the next neighbor. The result is the cross product. A positive cross product indicates a counterclockwise rotation from the previous neighbor to the current point and then to the next neighbor; a negative cross product indicates a clockwise rotation.

[0046] To determine if a point is inside the contour, a detection point is taken at a small detection distance from the current point on the angle bisector of the interior angle formed by the two line segments. This detection distance is on the order of 1 / 100 of the sampling interval. The determination of whether this detection point is inside the contour is based on the following rule: draw a ray from the detection point towards the positive horizontal axis, and count the number of intersections between this ray and the contour boundary curve. If the number of intersections is odd, the point is inside; if it is even, it is outside. If the detection point is inside, the interior angle is the smallest angle obtained in the first step. The rotation direction determines whether the smallest angle belongs to the interior or exterior side. If the detection point is outside, the interior angle... The value of 360 degrees minus the smallest included angle is the included angle on the inner side. Boundary points with interior angles greater than 180 degrees are marked as concave points. The geometric meaning of a concave point is that the contour boundary is recessed inward at this point, forming a gap structure. For each boundary point marked as a concave point, connect it one by one with all other boundary points in the closed boundary point sequence except itself using straight line segments. Each connecting line is called an initial connecting line segment. Perform internality verification on each initial connecting line segment. Set a set of detection points evenly on the initial connecting line segment at a preset verification interval. The verification interval is on the order of 1 / 10 of the sampling interval to fully cover the entire line segment. Use the aforementioned ray method to determine whether each detection point is located inside the contour. If all detection points are located inside, the initial connecting line segment passes the verification and is upgraded to a candidate segmentation line segment. If any detection point is located outside or exactly on the boundary, the line segment is discarded.

[0047] All candidate segmentation segments are subjected to intersection screening to eliminate redundancy. All candidate segmentation segments are paired up, and each pair of segments is checked for intersection at non-endpoints. The detection rules are as follows: Let the endpoints of the first segment be A and B, and the endpoints of the second segment be C and D. Using A as a reference, calculate the cross product of the vector from A to C and the vector from A to B, and the cross product of the vector from A to D and the vector from A to B. If the signs of the two cross product values ​​are positive and negative respectively, then C and D are on opposite sides of the line connecting A and B. Similarly, using C as a reference, check whether A and B are on opposite sides of the line connecting C and D. If both conditions are met, the two segments intersect internally. If two candidate segmentation segments share an endpoint, the endpoints coincide and are not counted as intersections. When a pair of segments intersects internally, calculate the lengths of the two segments, i.e., the Euclidean distance between the endpoints, and keep the shorter one and delete the longer one. The Euclidean distance is calculated by taking the square root of the sum of the squares of the differences in the horizontal and vertical coordinates of the endpoints.

[0048] The candidate segmentation segments retained after interoperability screening are the valid segmentation segments. The original boundary segments are merged with all valid segmentation segments to form a segment pool for dividing closed sub-regions. The division operation rules are as follows: Randomly select an unassigned segment from the segment pool as the starting edge, and walk along that edge to the ending point. At the ending point, select the segment from the unassigned segments that forms the smallest right turn angle with the current direction of travel as the next edge. Continue walking until returning to the starting point to form a closed loop. The area enclosed by this loop is a closed sub-region, and all segments traversed are marked as assigned. Repeat this process until all segments in the segment pool are assigned to a sub-region.

[0049] For each closed sub-region, a convexity test is performed. The test rule is as follows: take all vertices on the perimeter of the sub-region, and calculate the interior angles at each vertex sequentially along the perimeter's circumferential direction, using the same calculation method as in concave point identification. If all interior angles are not greater than 180 degrees, the sub-region is a convex sub-region and is directly used as a partitioning unit. If there are vertices with interior angles greater than 180 degrees, the perimeter of the sub-region is used as a new sequence of boundary points, and the operations of concave point identification, candidate segmentation line generation, effective segmentation line filtering, and region partitioning are repeated until all the partitioned sub-regions satisfy the convexity condition. Finally, all the non-overlapping convex sub-regions together form a partitioning unit set.

[0050] Take all boundary vertices on the current partitioned unit to form a two-dimensional point set; randomly shuffle the points in the set, which only changes the processing order and does not affect the final result; take the first two points in the shuffled set, and use the midpoint of the line connecting these two points as the initial center of a circle. The horizontal axis coordinate of the center is equal to half the sum of the horizontal axis coordinates of the two points, and the vertical axis coordinate of the center is equal to half the sum of the vertical axis coordinates of the two points. Use half the distance between the two points as the initial radius; the distance between the two points is the square root of the sum of the squares of the differences in the horizontal and vertical coordinates of the two points; start processing from the third point in the shuffled set and proceed point by point; for the current point being processed, calculate its Euclidean distance to the current center of the circle. If this distance is less than or equal to the current radius plus a small tolerance, the current circle already contains the point and no adjustment is made; the small tolerance is on the order of floating-point arithmetic precision. If the distance is greater than the current radius plus a small tolerance, the point is added to a boundary control point set and the circle is recalculated. The rules for calculating the circle using the boundary control point set are as follows: If the set contains 2 points, the circle is calculated using the line connecting the two points as the diameter; if it contains 3 points, it is first determined whether the three points are approximately collinear. The method for determining this is to calculate the absolute value of the difference between the slope of the line connecting the first two points and the slope of the line connecting the last two points. If it is less than a collinearity threshold, it is considered collinear, and the circle is still calculated using the two farthest points as the diameter; if they are not collinear, the circle is calculated using the circumcenter of the triangle formed by the three points as the center. The method for calculating the circumcenter of a triangle is as follows: take the perpendicular bisectors of any two sides of the triangle. The perpendicular bisector is a straight line passing through the midpoint of the side and perpendicular to the side. The intersection of the two perpendicular bisectors is the circumcenter; the distance from the circumcenter to any one of the three vertices is the radius.

[0051] After updating the circle parameters, continue processing subsequent points. Once all points in the point set have been processed, the current circle is the minimum circumcircle of the subdivision unit. Record its center x-axis coordinate, center y-axis coordinate, and radius. Perform the above calculation on all subdivision units and summarize the parameters of all minimum circumcircles into a list. Each element contains three values: two coordinate components of the center and the radius. This list is the description of the overall dynamic safety boundary at the current moment.

[0052] In a preferred embodiment of the present invention, step 3 is performed as follows: The parameters of all minimum circumcircles are read from the overall dynamic safety boundary description: there are K circles in total, and the x-axis coordinates and y-axis coordinates of the center of the k-th circle, as well as the radius k. For each obstacle point j in the obstacle space point set, all K minimum circumcircles are traversed, and the safety distance is calculated for each combination. The calculation rule is as follows: first, subtract the x-axis coordinate of obstacle point j from the x-axis coordinate of the center k and then square the result; then, subtract the y-axis coordinate of obstacle point j from the y-axis coordinate of the center k and then square the result; add the two squared values ​​and take the square root to obtain the Euclidean distance from obstacle point j to the center k; subtract the radius k from this Euclidean distance to obtain the safety distance of obstacle point j relative to the profile element k; a positive safety distance indicates that the obstacle point is outside the circle, and the larger the value, the safer it is; a zero safety distance indicates that the obstacle point is exactly on the circumference; a negative safety distance indicates that the obstacle point has entered the circle, and its absolute value represents the depth of intrusion.

[0053] In a double-loop loop, a fast filter is first performed to reduce the actual computational load: if the absolute value of the difference between the x-axis coordinate of obstacle point j and the x-axis coordinate of the center k is greater than the radius k plus a preset upper limit distance, then the combination is skipped; if the absolute value of the difference in the y-axis coordinate also exceeds this limit, it is also skipped. The upper limit distance is taken as several times the highest threshold among the various threshold levels; exceeding this limit has no actual impact on the safety judgment. After all combinations have been calculated, all safety distance values ​​are iterated and compared, and the minimum value is taken as the current safety distance. If all combinations are skipped, the current safety distance is taken as the upper limit distance.

[0054] The current safe distance is compared with preset multi-level thresholds to determine the threshold range and thus the safety level. The preset multi-level thresholds are a sequence of boundary values ​​from smallest to largest. The following explains the magnitude range and the basis for the values ​​of each threshold level: The first-level threshold, which is the minimum threshold value, corresponds to the shortest safe distance required from the detection of an obstacle to the complete stop of the entire machine after emergency braking. Its basic value is calculated as follows: take the maximum design speed of the chassis, multiply it by the total response time from the issuance of the braking command to the complete locking of the brakes to obtain the travel distance during the response period; take the square of the maximum design speed of the chassis, divide it by twice the maximum braking deceleration to obtain the travel distance during the braking period; add the travel distance during the response period to the travel distance during the braking period to obtain the total emergency braking distance; then add a safety margin to this total distance, the value of which is a fixed proportion of the total emergency braking distance, between 0.2 and 0.5; the final value of the first-level threshold is on the order of several hundred millimeters for small low-speed composite robots and several thousand millimeters for large high-speed composite robots.

[0055] The second-level threshold is calculated by adding a sensing and processing delay distance to the first-level threshold. This delay distance is calculated as follows: first, determine the sensor's detection and transmission delay time, including the total time from sensor sensing an obstacle to the data being read by the control system via the communication bus; then, determine the duration of one control cycle of the system; add the delay time to the duration of one control cycle to obtain the total delay time; multiply the total delay time by the maximum chassis linear speed to obtain the delay distance. Add the first-level threshold to the delay distance to obtain the second-level threshold. The magnitude of the second-level threshold is typically 1.5 to 3 times that of the first-level threshold.

[0056] The third-level threshold is calculated by adding a manual intervention distance to the second-level threshold. This manual intervention distance is calculated as follows: The standard reaction time required for the operator to notice the warning and make a manual intervention decision is taken; this reaction time is determined according to human factors engineering standards and is on the order of 1 to 3 seconds. The total intervention time is obtained by adding the communication delay time from receiving the instruction to the instruction taking effect at the operating terminal. The intervention distance is obtained by multiplying the total intervention time by the chassis's normal operating speed. The third-level threshold is then added to the second-level threshold. The magnitude of the third-level threshold is typically 1.5 to 3 times that of the second-level threshold.

[0057] If the application scenario requires finer-grained security grading, intermediate thresholds can be inserted between the above-mentioned threshold levels at geometric or arithmetic intervals. For geometric insertion, the geometric mean of two adjacent threshold levels (the square root of their product) is used; for arithmetic insertion, the arithmetic mean of two adjacent threshold levels (the sum of the two divided by 2) is used. The rule for comparing the current safe distance with each threshold level is as follows: starting from the first level (the threshold with the smallest value), if the current safe distance is less than or equal to the first-level threshold, it falls within the first-level threshold range, corresponding to the most dangerous level. If the current safe distance is greater than the first-level threshold, it is compared with the second-level threshold; if it is less than or equal to the second-level threshold, it falls within the second-level threshold range. This process continues upwards until the current safe distance is less than or equal to a certain threshold level, thus determining its current range. If the current safe distance is greater than all thresholds, it falls within the range of the highest-lowest threshold level, corresponding to the safest level. There is an ordered correspondence between threshold ranges and security levels: the lower the value of the range, the more dangerous the security level.

[0058] In a preferred embodiment of the present invention, step 4 is performed as follows: The highest safety level corresponds to the normal operating state of the equipment. The system will not impose any speed, displacement, or rotational motion constraints on the mobile chassis or multi-joint robotic arm. The audible and visual warning devices are in a silent standby state. The entire machine only maintains low power consumption and uninterrupted environmental safety monitoring to ensure the stable operation of the equipment under normal conditions.

[0059] The next safety level corresponds to a warning and response status. The system triggers the equipment's warning mechanism, controlling the yellow warning light to flash slowly, alternating between on and off over a few seconds, while simultaneously driving the buzzer to emit a low-volume, intermittent warning sound. At the same time, the equipment's operating terminal pushes warning details in real time, accurately displaying the location range of the obstacle that triggered the warning in the world coordinate system and the current real-time safe distance data. In this state, the composite robot maintains its original operating parameters and operates normally without any movement restrictions. Operators can combine the warning information pushed by the terminal to independently determine whether to carry out manual intervention and protection operations.

[0060] The intermediate safety level corresponds to the deceleration and protection operation state. The system activates the equipment speed limit control mechanism, sending a speed constraint command to the chassis controller to limit the real-time operating speed of the chassis to 1 / 3 to 1 / 2 of the normal operating speed. The specific speed limit ratio can be adjusted according to the on-site safety assessment results. Simultaneously, a joint motion constraint command is sent to the robotic arm controller, limiting the real-time rotational angular velocity of each joint to 1 / 3 to 1 / 2 of the normal maximum angular velocity. The equipment warning level is upgraded simultaneously, the yellow warning light switches to a sub-second rapid flashing mode, and the buzzer outputs a high-frequency, rapid, intermittent warning sound to enhance the on-site warning effect. When the system continuously monitors that the safe distance has risen above the warning level control standard, the equipment speed limit constraint is automatically lifted, and the equipment returns to normal operation.

[0061] A higher safety level corresponds to a pause standby protection state. The system triggers a smooth shutdown control logic, controlling the mobile chassis to continuously and uniformly decelerate at the equipment's rated maximum braking deceleration rate until the chassis speed drops completely to zero. After stopping, it remains locked in a zero-speed stationary state to prevent equipment slippage or abnormal movement. Simultaneously, shutdown constraint control is applied to each joint of the robotic arm, controlling all joints to smoothly decelerate at a constant maximum braking deceleration rate, gradually terminating all movements to ensure a smooth and shock-free shutdown process. The equipment warning mechanism has been further upgraded, with red warning lights flashing rapidly, a buzzer continuously outputting intermittent alarm sounds, and a prominent pause prompt interface popping up on the operating terminal to remind on-site personnel that the equipment has entered a safe standby state. The system continuously monitors changes in the on-site safe distance in real time. When the safe distance rises above the deceleration level control standard, the equipment automatically releases the locked standby state and resumes normal operation.

[0062] The most dangerous safety level corresponds to the emergency braking protection state. The system triggers the highest level of safety protection logic, immediately sending a power cut-off command to the chassis drive motor controller to disconnect the driver enable path. At the same time, it triggers the chassis electromagnetic brake locking mechanism, cutting off the brake's power-on holding signal and quickly locking the drive shaft using the brake's built-in spring structure, achieving an emergency stop for the chassis. Simultaneously, it performs the same-source protection operation on all joints of the robotic arm, uniformly cutting off the drive power and locking the braking structure, instantly terminating all movement. The equipment warning system reaches the highest warning level, the red warning light switches from flashing to continuous illumination, the buzzer outputs a continuous high-decibel alarm sound, and the operating terminal displays the highest priority full-screen emergency braking prompt, comprehensively alerting on-site personnel that the equipment has triggered emergency protection. In this state, the equipment cannot be automatically reset and restarted. On-site personnel must check and confirm that the working environment is safe before manually unlocking and restoring the equipment's operating permissions via the hardware reset button.

[0063] In addition to the standard protection and handling logic at fixed levels, the equipment can be extended to adapt to active obstacle avoidance and protection functions. When the equipment is in a deceleration protection state or a paused standby state, the system automatically starts a local path replanning program. Taking the current real-time position of the composite robot as the starting point and the preset work target point as the endpoint, it searches for the optimal passable path within the barrier-free grid area of ​​the site. During the path search process, two core cost parameters are comprehensively evaluated: one is the cumulative passage cost, which is the actual path length of the equipment from the starting point to the current grid; the other is the estimated heuristic cost, which is the straight-line distance from the current grid point to the work endpoint. The path search follows safety constraints throughout, ensuring that the distance between all passable grids on the planned path and the nearest surrounding obstacles always meets the preset safety standards. After the optimal passage path obtained is smoothed and optimized, it is synchronously sent to the chassis controller for execution, enabling the equipment to autonomously avoid obstacles and detour.

[0064] The entire hierarchical safety interlock control logic supports authorized personnel to dynamically adapt and adjust it online. It can optimize core configurations such as safety judgment thresholds, equipment deceleration ratios, warning light flashing frequency, and buzzer volume parameters corresponding to each level of risk in real time according to changes in work scenarios, equipment conditions, and safety regulations. All parameter adjustments take effect immediately without restarting the equipment or interrupting the work process.

[0065] After the handling process of this stage is completed, the system automatically records the complete operation data of this control cycle. The log records include the operation timestamp, the real-time safety level, the current environmental safety distance value, the types of protective actions that have been triggered, and the operation status feedback information of each actuator. After the data recording is completed, the system returns to the initial data acquisition stage and starts the full-process monitoring and protection judgment of the next control cycle, forming an uninterrupted, closed-loop operation safety protection process.

[0066] like Figure 2As shown, embodiments of the present invention also provide an operational protection system based on dynamic sweep envelope determination, comprising: The multi-source data acquisition module is used to acquire in real time motion state data of the composite robot's mobile chassis, joint state and pose data of the robotic arm, end-effector load state data, and perception data of the surrounding environment. The dynamic envelope modeling module is used to obtain the 3D motion sweep volume envelope within a preset time window based on the joint state and pose data of the robotic arm and the end-effector load state data; to obtain the chassis dynamic safety envelope based on the motion state data of the mobile chassis and the preset chassis body dimensions; to project and fuse the 3D motion sweep volume envelope and the chassis dynamic safety envelope onto the chassis driving reference plane to obtain a 2D composite safety boundary profile; to subdivide the 2D composite safety boundary profile to obtain multiple subdivided elements; to calculate the minimum circumcircle of each subdivided element, and to form the overall dynamic safety boundary description by all minimum circumcircles. The safety distance determination module converts the perceived data of the surrounding environment into a set of obstacle spatial points. Using the center of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, it determines the nearest geometric distance from the obstacle point to the circumference of the smallest circumcircle, which is taken as the safety distance of the obstacle point relative to the subdivision unit. The minimum value among all safety distances is taken as the current safety distance. Based on the comparison between the current safety distance and the preset multi-level thresholds, the current safety level is determined. The execution module is used to match and execute the hierarchical security interlock strategy corresponding to the security level, so as to perform the corresponding protective actions.

[0067] It should be noted that this system is a system corresponding to the above method. All implementation methods in the above method embodiments are applicable to this embodiment and can achieve the same technical effect.

[0068] Embodiments of the present invention also provide a computing device, including: a processor and a memory storing a computer program, wherein the computer program, when executed by the processor, performs the method described above. All implementations in the above method embodiments are applicable to this embodiment and can achieve the same technical effects.

[0069] Embodiments of the present invention also provide a computer-readable storage medium storing instructions that, when executed on a computer, cause the computer to perform the method described above. All implementations in the above method embodiments are applicable to this embodiment and can achieve the same technical effects.

[0070] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for operational protection based on dynamic sweep envelope determination, characterized in that, The method includes: Step 1: Real-time acquisition of motion state data of the composite robot's mobile chassis, joint state and pose data of the robotic arm, end-effector load state data, and perception data of the surrounding environment. Step 2: Based on the joint state and pose data of the robotic arm and the end-effector load state data, obtain the three-dimensional motion sweep volume envelope within a preset time window; based on the motion state data of the mobile chassis and the preset chassis body dimensions, obtain the chassis dynamic safety envelope; project the three-dimensional motion sweep volume envelope and the chassis dynamic safety envelope onto the chassis driving reference plane and fuse them to obtain a two-dimensional composite safety boundary profile; subdivide the two-dimensional composite safety boundary profile to obtain multiple subdivided units; calculate the minimum circumcircle of each subdivided unit, and the overall dynamic safety boundary description is composed of all minimum circumcircles. Step 3: Convert the perceived data of the surrounding environment into a set of obstacle spatial points. Using the center of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, determine the shortest geometric distance from the obstacle point to the circumference of the smallest circumcircle, which is used as the safe distance of the obstacle point relative to the subdivision unit. Take the minimum value among all safe distances as the current safe distance. Determine the current safety level based on the comparison result between the current safe distance and the preset multi-level threshold. Step 4: Based on the security level, match and execute the graded security interlock strategy corresponding to the security level to perform the corresponding protective actions.

2. The operation protection method based on dynamic sweep envelope determination according to claim 1, characterized in that, Step 2 includes: Based on the joint state and pose data and end-effector load state data of the robotic arm, the motion sweep path of each link and end-effector load of the robotic arm is predicted within a preset time window. The three-dimensional space area continuously swept by the motion sweep path is taken as the three-dimensional motion sweep volume envelope. Based on the motion state data of the mobile chassis and the preset chassis body size, the driving trajectory of the mobile chassis is predicted within a preset time window. The planar area continuously occupied by the driving trajectory is taken as the dynamic safety envelope of the chassis. The three-dimensional motion sweep volume envelope is vertically projected onto the chassis driving reference plane to obtain the first projection area. The chassis dynamic safety envelope is vertically projected onto the chassis driving reference plane to obtain the second projection area. The first and second projection areas are then fused to obtain the two-dimensional composite safety boundary contour. The two-dimensional composite safety boundary contour is convexly subdivided to obtain multiple non-overlapping subdivided elements that cover the two-dimensional composite safety boundary contour. Calculate the minimum circumcircle surrounding the partitioned element, and use the set of all minimum circumcircles as the overall dynamic safety boundary description.

3. The operation protection method based on dynamic sweep envelope determination according to claim 2, characterized in that, The three-dimensional motion sweep volume envelope is vertically projected onto the chassis driving reference plane to obtain the first projection region. The chassis dynamic safety envelope is vertically projected onto the chassis driving reference plane to obtain the second projection region. The first and second projection regions are then fused to obtain the two-dimensional composite safety boundary contour, including: Extract the outermost contour point set of the 3D motion sweep volume envelope in the direction perpendicular to the chassis driving reference plane, map the outermost contour point set to the chassis driving reference plane in the vertical direction, and take the closed plane region enclosed by the mapped point set as the first projection region; the plane region occupied by the chassis dynamic safety envelope is directly determined as the second projection region. The planar regions covered by the first projection region and the planar regions covered by the second projection region are merged, and the resulting continuous planar region is taken as the two-dimensional composite safety boundary profile.

4. The operation protection method based on dynamic sweep envelope determination according to claim 3, characterized in that, The two-dimensional composite safety boundary contour is convexly subdivided to obtain multiple non-overlapping subdivided elements that cover the two-dimensional composite safety boundary contour, including: Extract the sequence of closed boundary points of the two-dimensional composite safety boundary profile. Along the circumferential direction of the closed boundary point sequence, calculate the interior angle formed by the two adjacent boundary line segments at each boundary point in turn. Boundary points with interior angles greater than 180 degrees are regarded as concave points. Based on the identified concave points, within the two-dimensional composite safety boundary profile, connecting line segments are drawn from each concave point to other boundary points in the closed boundary point sequence. The two-dimensional composite safety boundary profile is divided into multiple convex sub-regions using these connecting line segments. All convex sub-regions do not overlap and, when stitched together, completely cover the two-dimensional composite safety boundary profile. Each convex sub-region is treated as a partitioning unit.

5. The operation protection method based on dynamic sweep envelope determination according to claim 4, characterized in that, Based on the identified concave points, within the two-dimensional composite safety boundary contour, connecting line segments are drawn from each concave point to other boundary points in the closed boundary point sequence. These connecting line segments divide the two-dimensional composite safety boundary contour into multiple convex sub-regions. All convex sub-regions are non-overlapping and, when stitched together, completely cover the two-dimensional composite safety boundary contour. Each convex sub-region is considered as a partitioning unit, including: Connect the concave point to each boundary point in the closed boundary point sequence except for the concave point itself to generate multiple initial connecting line segments; determine whether all points on the initial connecting line segments except for the two end boundary points are located in the internal region of the two-dimensional composite safety boundary contour. If they are all located in the internal region, the initial connecting line segments are determined as candidate segmentation line segments. Intersection detection is performed on all candidate segmentation segments, redundant segments that intersect with other candidate segmentation segments are deleted, and the remaining candidate segmentation segments are taken as valid segmentation segments; the internal region of the two-dimensional composite safety boundary contour is divided into multiple closed sub-regions, such that the perimeter of each closed sub-region is formed by connecting a part of the original boundary segment and a part of the valid segmentation segment end to end; Obtain all interior angles of each closed sub-region one by one. If all interior angles of a closed sub-region are no greater than 180 degrees, then the closed sub-region is regarded as a convex sub-region. All convex sub-regions are regarded as subdivision units.

6. The operation protection method based on dynamic sweep envelope determination according to claim 5, characterized in that, Calculate the minimum circumcircle surrounding the meshed elements, and use the set of all minimum circumcircles as the overall dynamic safety boundary description, including: Obtain the boundary vertex sequence of each subdivision unit; using the boundary vertex sequence as input, solve for a circle such that all vertices in the boundary vertex sequence are located on or inside the circumference of the circle, and the radius of the circle is the smallest among all circles that satisfy the above conditions. Use the solved circle as the minimum circumcircle corresponding to the subdivision unit; combine all the minimum circumcircles corresponding to all subdivision units together to obtain the overall dynamic safety boundary description.

7. The operation protection method based on dynamic sweep envelope determination according to claim 6, characterized in that, Step 3 includes: Based on the perception data of the surrounding environment, the spatial location information of obstacles is extracted, and all the extracted obstacle spatial location points are combined into an obstacle spatial point set. Obtain the center position and radius of each minimum circumcircle contained in the overall dynamic safety boundary description; for each obstacle point in the obstacle space point set, calculate the Euclidean distance between the obstacle point and the center position of each minimum circumcircle in turn, subtract the radius of the corresponding minimum circumcircle from the calculated Euclidean distance to obtain the shortest geometric distance from the obstacle point to the circumference of the corresponding minimum circumcircle, and use the shortest geometric distance as the safety distance of the obstacle point relative to the corresponding subdivision unit; From all safe distances, the one with the smallest value is taken as the current safe distance; based on the comparison results between the current safe distance and the thresholds of each level in the preset multi-level thresholds, the threshold interval in which the current safe distance is located is determined, and the current safety level is determined based on the threshold interval.

8. A job protection system based on dynamic sweep envelope determination, wherein the system implements the method as described in any one of claims 1 to 7, characterized in that, include: The multi-source data acquisition module is used to acquire in real time motion state data of the composite robot's mobile chassis, joint state and pose data of the robotic arm, end-effector load state data, and perception data of the surrounding environment. The dynamic envelope modeling module is used to obtain the three-dimensional motion sweep volume envelope within a preset time window based on the joint state and pose data of the robotic arm and the end-effector load state data. Based on the motion state data of the mobile chassis and the preset chassis body dimensions, the dynamic safety envelope of the chassis is obtained; the three-dimensional motion sweep volume envelope and the dynamic safety envelope of the chassis are projected onto the chassis driving reference plane and fused to obtain the two-dimensional composite safety boundary profile. The two-dimensional composite safety boundary profile is subdivided to obtain multiple subdivided elements; the minimum circumcircle of each subdivided element is calculated, and the overall dynamic safety boundary description is formed by all minimum circumcircles. The safety distance determination module converts the perceived data of the surrounding environment into a set of obstacle spatial points. Using the center of the smallest circumcircle as a reference and the radius of the smallest circumcircle as a constraint, it determines the nearest geometric distance from the obstacle point to the circumference of the smallest circumcircle, which is taken as the safety distance of the obstacle point relative to the subdivision unit. The minimum value among all safety distances is taken as the current safety distance. Based on the comparison between the current safety distance and the preset multi-level thresholds, the current safety level is determined. The execution module is used to match and execute the hierarchical security interlock strategy corresponding to the security level, so as to perform the corresponding protective actions.

9. A computing device, characterized in that, include: One or more processors; A storage device for storing one or more programs, which, when executed by one or more processors, cause the one or more processors to implement the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a program that, when executed by a processor, implements the method as described in any one of claims 1 to 7.

Images