Safety detection method and system for robot based on visual detection

Abstract

The application discloses a mechanical arm safety detection method and system based on visual detection, and belongs to the technical field of visual detection. The method specifically comprises the following steps: controlling the mechanical arm to perform a preset micro-motion operation, dynamically correcting the coordinate conversion relationship between a visual collection device and the mechanical arm, extracting a visible target occupied area based on the corrected depth image, dividing a blind area unit in combination with the historical motion trajectory of the visible target, deducing a candidate occupied path and generating a blind area compensation occupied area, further constructing a motion sweeping unit chain and a braking sweeping unit chain according to the current pose, the current speed, the control delay and the braking parameters of the mechanical arm, and performing time sequence matching on the motion sweeping unit chain, the braking sweeping unit chain, the visible target occupied area and the blind area compensation occupied area to determine the overlapping space unit, the first overlapping order and the collision risk type; the influence of the position drift of the visual collection device on the spatial detection result is reduced, and the motion intrusion risk, the braking continuation risk and the blind area pending risk can be distinguished.

Description

Technical Field

[0001] This invention belongs to the field of visual inspection technology, specifically a method and system for safety inspection of robotic arms based on visual inspection. Background Technology

[0002] With the development of automated production lines, flexible assembly systems, and intelligent warehousing equipment, robotic arms are widely used in workpiece gripping, handling, assembly, inspection, and sorting. During operation, robotic arms typically feature high speed, large range of motion, and complex joint linkages. When there are moving targets, temporary objects, or workpieces whose positions change within their working area, it is necessary to promptly determine the spatial relationship between the robotic arm and surrounding targets.

[0003] Current methods for detecting the safety of robotic arms mainly include setting up mechanical fences, deploying optical gratings, or proximity sensors. These methods primarily rely on the instantaneous distance between the robotic arm and the target, whether the enclosed areas intersect, or whether the target enters a pre-set warning zone as the basis for risk assessment. Existing methods often use the robotic arm's current pose, planned trajectory, or instantaneous safe distance as risk assessment criteria, rarely incorporating control delays, the braking process of each joint, and the spatial range traversed by the robotic arm before it stops into a unified assessment. When a target moves from a visible area to an obscured area, or when the area occupied by the target overlaps with the spatial range formed during the robotic arm's braking process, problems such as interrupted spatial judgment, missing blind spot information, and mismatch between the stopping timing and the actual braking process can easily occur. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention proposes a visual detection-based safety detection method and system for robotic arms. This system can perform in-process correction of visual coordinate relationships, compensate for target distribution within occluded areas, and determine collision risks by combining the robotic arm's movement and braking processes.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A vision-based safety inspection method for robotic arms is applied to robotic arm operation systems that include the robotic arm, a vision acquisition device, a joint state acquisition device, and a safety controller.

[0007] The system acquires depth images and joint pose data of the robot's working area, identifies the robot's body area, and corrects the coordinate transformation relationship between the vision acquisition device and the robot based on the deviation between the actual and theoretical positions of the robot's body area.

[0008] Based on the corrected coordinate transformation relationship, the visible target occupied area in the working area is obtained from the depth image. Based on the occupancy area formed by the robot body, workpiece or fixed facility and the historical motion state of the visible target, the distribution range of the visible target in the occupancy area is predicted, and the blind spot compensation occupied area is generated.

[0009] Based on the current pose, current speed, control delay, and braking parameters of the robotic arm, determine the motion sweep area formed when the robotic arm continues to move and the braking sweep area formed after receiving the stop command. Based on the overlap between the motion sweep area and the braking sweep area and the area occupied by the visible target and the blind zone compensation area, determine the collision risk of the robotic arm.

[0010] When the collision risk reaches the stop threshold, the robot arm is controlled to stop moving. When the collision risk is caused by the blind spot compensation area and has not reached the stop threshold, the robot arm is controlled to move along the preset safety trajectory and perform visual detection again. Once the collision risk is lower than the safety threshold, the robot arm is allowed to continue performing the operation.

[0011] Specifically, the step of correcting the coordinate transformation relationship between the vision acquisition device and the robot arm based on the deviation between the actual and theoretical positions of the robot arm's body area includes:

[0012] The robot body area is divided into multiple body partitions, and each body partition is controlled to perform micro-displacement movements in a preset order, while simultaneously acquiring images of the partition area and the joint pose data of the partition.

[0013] The actual displacement trajectory of each body partition is determined based on the partitioned region image, and the corresponding theoretical displacement trajectory is determined based on the partitioned joint pose data and the manipulator kinematic model.

[0014] Based on the execution sequence of micro-displacement actions, the actual displacement trajectory is paired with the theoretical displacement trajectory to identify abnormal trajectory segments that do not match.

[0015] Based on the motion sequence of adjacent body partitions, identify and remove trajectory segments caused by occlusion from the abnormal trajectory segments, and generate a trajectory deviation sequence for each body partition based on the deviation between the remaining actual displacement trajectory and the theoretical displacement trajectory.

[0016] Based on the trajectory deviation sequence, the coordinate deviations common to each body partition are first corrected, and then the corresponding local coordinate deviations are corrected in the order of the distances between each body partition and the robot arm base to obtain the corrected coordinate transformation relationship.

[0017] Specifically, based on the trajectory deviation sequence, the common coordinate deviations of each body partition are first corrected, and then the corresponding local coordinate deviations are corrected in the order of the distances between each body partition and the robot arm base, to obtain the corrected coordinate transformation relationship, including:

[0018] The trajectory deviation sequence is sorted according to the distance between each body partition and the robot arm base, and common trajectory deviations with the same direction are extracted from multiple body partitions.

[0019] The current coordinate transformation relationship is adjusted according to the common trajectory deviation, and the theoretical displacement trajectory of each body partition is regenerated based on the adjusted coordinate transformation relationship.

[0020] Based on the regenerated theoretical displacement trajectory, the remaining trajectory deviation of each body section is determined, and the corresponding local correction amount is determined sequentially according to the distance between each body section and the robot arm base from near to far.

[0021] The local correction values ​​are verified in order from farthest to nearest. Local correction values ​​that are duplicated with the common trajectory deviation are removed. The retained local correction values ​​are then merged with the adjusted coordinate transformation relationship to obtain the corrected coordinate transformation relationship.

[0022] Specifically, the area occupied by the generated blind spot compensation includes:

[0023] Based on the corrected coordinate transformation relationship, the depth image is transformed to the robot's base coordinate system, the depth points corresponding to the robot body, workpiece and fixed facilities are removed, the area occupied by the visible target is obtained, and the historical motion trajectory of the visible target is generated based on the continuous depth image.

[0024] Based on the obstruction boundary formed by the robot body, workpiece, and fixed facilities, the obstruction area extending from the obstruction boundary into the working area is determined, and the obstruction area is divided into multiple blind zone units according to the spatial connectivity.

[0025] The end position of the historical motion trajectory is matched with the entrance position of each blind spot unit to determine the target blind spot unit associated with the historical motion trajectory;

[0026] Based on the direction and sequence of movement of the historical trajectory, the path is extended from the entrance position of the target blind zone unit to the adjacent blind zone unit, and the blind zone unit adjacent to the visible area is traced back to the entrance position. The blind zone unit whose path extension result and the reverse tracing result coincide is determined as the candidate occupied unit.

[0027] According to the order in which each historical movement trajectory enters the occupied area, the corresponding candidate occupied units are merged, and the merged candidate occupied units are determined as the blind zone compensation occupied area.

[0028] Specifically, determining the blind zone units where the path extension results and reverse backtracking results coincide as candidate occupied units includes:

[0029] Based on the changes in the direction of motion before entering the target blind zone unit according to the historical motion trajectory, an entry direction sequence is generated;

[0030] Starting with the target blind zone unit, the system extends to adjacent blind zone units according to the entrance direction sequence, and records the blind zone units passed through in sequence to form a forward unit chain.

[0031] Starting from the end blind zone unit adjacent to the visible area in the forward unit chain, backtracking is performed towards the target blind zone unit in the reverse order of the forward unit chain to form a reverse unit chain;

[0032] The blind zone cells that the forward cell chain and the reverse cell chain both pass through are identified as candidate occupied cells.

[0033] Specifically, the step of merging the corresponding candidate occupying units according to the order in which each historical movement trajectory enters the occlusion area, and determining the merged candidate occupying units as the blind spot compensation occupying area, includes:

[0034] According to the order in which each historical movement trajectory enters the occupied area, the corresponding candidate occupied units are sorted to form multiple candidate occupied unit chains;

[0035] According to the sorting results, each candidate occupying unit chain is written into the occluded area in sequence;

[0036] When the current candidate occupied cell chain overlaps with the written candidate occupied cell chain, non-overlapping cells with the same direction and continuous arrangement are connected to the written candidate occupied cell chain, and overlapping cells with inconsistent direction or discontinuous arrangement are transferred to adjacent unwritten blind zone cells.

[0037] Delete candidate occupied units that are not connected to the entrance of the corresponding target blind zone unit, and merge the remaining candidate occupied units to obtain the blind zone compensation occupied area.

[0038] Specifically, based on the current pose, current speed, control delay, and braking parameters of the robotic arm, the motion sweep area formed when the robotic arm continues to move and the braking sweep area formed after receiving a stop command are determined. Furthermore, based on the overlap between the motion sweep area and the braking sweep area and the area occupied by the visible target and the blind spot compensation area, the collision risk of the robotic arm is determined, including:

[0039] Based on the current pose, current speed, control delay, and braking parameters of the robot arm, the spatial units that the robot arm will pass through in sequence as it continues to move are determined, forming a chain of motion sweeping units;

[0040] The braking start state of the robot is determined based on the control delay, and the spatial units that the robot passes through in sequence before stopping are determined based on the braking parameters, forming a braking sweep unit chain connected to the motion sweep unit chain.

[0041] According to the historical movement sequence of visible targets and the extension sequence of candidate occupied units, the spatial units in the occupied area of ​​visible targets and the occupied area of ​​blind zone compensation are sorted to form an occupied unit sequence;

[0042] The motion sweep unit chain and the braking sweep unit chain are matched with the occupied unit sequence to determine the overlapping spatial units and their first overlap order.

[0043] The collision risk of the robot is determined based on the position of the overlapping spatial unit in the motion sweep unit chain or braking sweep unit chain, the first overlap order, and the corresponding occupied area type.

[0044] Specifically, the step of matching the motion sweep unit chain and the braking sweep unit chain with the occupied unit sequence to determine the overlapping spatial units and their first overlap order includes:

[0045] Each spatial unit is assigned a corresponding sequence identifier according to its arrangement in the motion sweep unit chain, braking sweep unit chain, and occupied unit sequence.

[0046] According to the arrangement order of the motion sweep unit chain, the spatial units are matched with the occupied unit sequence, and the first matching unit that overlaps is recorded.

[0047] According to the reverse arrangement order of the braking sweep unit chain, the spatial units therein are matched with the first matching unit to determine the second matching unit that coincides with both the motion sweep unit chain and the braking sweep unit chain.

[0048] The second matching units are sorted according to the motion sequence identifier, the corresponding spatial units are determined as overlapping spatial units, and the motion sequence of the first sorted second matching unit is determined as the first overlapping sequence.

[0049] Specifically, based on the position of the overlapping spatial unit within the motion sweep unit chain or braking sweep unit chain, the order of the first overlap, and the corresponding occupied area type, the collision risk of the robot arm is determined, including:

[0050] Based on the position of each overlapping spatial unit in the motion sweep unit chain or braking sweep unit chain, set the corresponding stage identifier and record the first overlap order and occupied area type.

[0051] Arrange the overlapping spatial units according to the first overlapping order, and merge the overlapping spatial units that are consecutive in position, have the same stage identifier and occupy the same area type into overlapping event segments.

[0052] Along the arrangement direction of the motion sweep unit chain and the braking sweep unit chain, the stage changes and occupied area type changes of each overlapping event segment are determined sequentially to form a collision transmission sequence.

[0053] Based on the stage identifier corresponding to the first overlapping event segment, whether the collision transmission sequence extends to the braking sweep unit chain, and the corresponding occupied area type, the collision risk of the manipulator is determined as motion intrusion risk, braking continuation risk, or blind zone undetermined risk.

[0054] The vision-based robotic arm safety detection system is used to implement the vision-based robotic arm safety detection method, and includes: a coordinate correction module, a blind spot compensation module, a risk assessment module, and a safety control module.

[0055] The coordinate correction module is used to acquire depth images and joint pose data of the working area of ​​the robot arm, identify the robot arm body area, and correct the coordinate transformation relationship between the vision acquisition device and the robot arm based on the deviation between the actual position and the theoretical position of the robot arm body area.

[0056] The blind spot compensation module is used to obtain the visible target occupied area in the working area from the depth image based on the corrected coordinate transformation relationship, and predict the distribution range of the visible target in the occupancy area according to the occupancy area formed by the robot body, workpiece or fixed facility and the historical motion state of the visible target, and generate the blind spot compensation occupied area.

[0057] The risk assessment module is used to determine the motion sweep area formed when the robot continues to move and the braking sweep area formed after receiving the stop command, based on the robot's current pose, current speed, control delay and braking parameters. It also determines the collision risk of the robot based on the overlap between the motion sweep area and the braking sweep area and the visible target occupied area and the blind zone compensation occupied area.

[0058] The safety control module is used to control the robot to stop moving when the collision risk reaches the stop threshold, and to control the robot to move along a preset safety trajectory and re-perform visual detection when the collision risk is caused by the blind spot compensation area and has not reached the stop threshold. After the collision risk is lower than the safety threshold, the robot is allowed to continue to perform the operation.

[0059] Compared with the prior art, the beneficial effects of the present invention are:

[0060] This invention proposes a visual detection-based method and system for safe detection of robotic arms. By dynamically correcting the coordinate transformation relationship between the visual acquisition device and the robotic arm, and combining the historical motion trajectories of visible targets to generate blind spot compensation areas, the system constructs motion sweep areas and braking sweep areas based on the current motion state, control delay, and braking parameters of the robotic arm. This determines the overlap and collision risk between the robotic arm and each occupied area. Therefore, the impact of visual acquisition device position drift on spatial detection results can be reduced, missing target distribution information within occluded areas can be supplemented, and the robotic arm's continued movement, control delay, and braking processes can be integrated into a unified risk assessment chain. This avoids detection interruptions and risk omissions caused by relying solely on instantaneous distance or current image data, and enables the differentiation of different risks. Attached Figure Description

[0061] Figure 1 A flowchart of the vision-based robotic arm safety detection method provided by the present invention;

[0062] Figure 2 A schematic diagram of the robotic arm operating system provided by the present invention;

[0063] Figure 3 This is a schematic diagram of the partitioning of the robotic arm body provided by the present invention;

[0064] Figure 4 The architecture diagram of the vision-based robotic arm safety detection system provided by this invention;

[0065] In the diagram: 1. Robotic arm; 2. Vision acquisition device; 3. Joint status acquisition device; 4. Safety controller; 5. Conveying device; 6. Positioning stage; 7. Workpiece; 8. Fixing facility; 9. Visible target; 10. Blind spot area; 11. Third body section; 12. Second body section; 13. First body section; 14. Fourth body section; 15. Fifth body section; 16. Sixth body section; 17. End effector section. Detailed Implementation

[0066] The present application will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present application, but do not limit the present application in any way. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the present application. These all fall within the protection scope of the present application.

[0067] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0068] It should be noted that, unless there is a conflict, the various features in the embodiments of this application can be combined with each other, all of which are within the protection scope of this application. Furthermore, although functional modules are divided in the device schematic diagram and a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order than the module division in the device or the order in the flowchart. In addition, the terms "first," "second," and "third" used in this application do not limit the data or execution order, but only distinguish identical or similar items with essentially the same function and effect.

[0069] Unless otherwise defined, all technical and scientific terms used in this specification have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the scope of this application. The term "and / or" as used in this specification includes any and all combinations of one or more of the associated listed items.

[0070] Example 1

[0071] Please see Figure 1 This invention provides an embodiment of a vision-based safety detection method for a robotic arm. This method is applied to a robotic arm operating system, which includes a robotic arm, a vision acquisition device, a joint status acquisition device, and a safety controller. The robotic arm is an industrial robotic arm with six rotating joints, and its end effector is installed at its end for gripping workpieces to be transported. The vision acquisition device is a depth camera capable of continuously outputting depth images, positioned above the robotic arm's working area, ensuring that its acquisition range covers at least the main motion area of ​​the robotic arm, the workpiece picking and placing area, and the workpiece conveying area. The joint status acquisition device acquires the joint position, joint speed, and joint operating status of each joint of the robotic arm. The safety controller interacts with the robotic arm controller, the vision acquisition device, and the joint status acquisition device to perform coordinate correction, occupied area generation, swept area generation, collision risk assessment, and robotic arm motion control.

[0072] In this embodiment, the robot arm is set on one side of the worktable, which is equipped with a workpiece storage rack, a positioning platform, and a conveying device. The robot arm picks up the workpiece from the conveying device according to a preset work trajectory and moves the gripped workpiece to the positioning platform. The robot arm body, the end effector, the gripped workpiece, the workpiece storage rack, the positioning platform, and the conveying device may all form obstructions in the acquisition direction of the vision acquisition device. In addition to the above-mentioned fixed facilities and the robot arm itself, there may also be material carts, turnover boxes, or other movable targets moving along a predetermined channel in the work area.

[0073] The specific steps include the following:

[0074] Step S1: Acquire depth images and joint pose data of the working area of ​​the robot arm, identify the robot arm body area, and correct the coordinate transformation relationship between the vision acquisition device and the robot arm based on the deviation between the actual position and the theoretical position of the robot arm body area.

[0075] First, a base coordinate system for the robotic arm, a coordinate system for the vision acquisition device, and a set of spatial units for the working area are established. The set of spatial units for the working area is obtained by three-dimensional discretizing the robotic arm's working area. Each spatial unit corresponds to a spatial range within the robotic arm's base coordinate system. The size of the spatial unit is set according to the depth resolution of the vision acquisition device, the positioning accuracy of the robotic arm, and the external dimensions of the end effector. In one specific embodiment, spatial units closer to the robotic arm's movement area are smaller, while those farther away are larger. In subsequent processing, depth points, robotic arm body partitions, occupied areas, areas occupied by visible targets, and the robotic arm's sweeping area are converted into corresponding sets of spatial units.

[0076] Before the robotic arm begins its formal handling operations, it reads the initial coordinate transformation relationship obtained during the installation of the vision acquisition device. This initial coordinate transformation relationship is used to transform the depth point in the vision acquisition device's coordinate system to the robotic arm's base coordinate system. Considering that the installation state of the vision acquisition device may change due to mechanical vibration, support displacement, or maintenance operations, this embodiment does not directly use the initial coordinate transformation relationship as a fixed transformation relationship throughout the entire operation process. Instead, it corrects the coordinate transformation relationship during the robotic arm's operation using the actual and theoretical positions of the robotic arm's body area.

[0077] Specifically, when the robot arm is in the initial operation state, the workpiece pick-up and place interval state, or the current collision risk is lower than the preset correction threshold, the preset correction threshold is determined based on the micro-sweep area corresponding to the micro-displacement action performed by the robot arm, and the coordinate transformation relationship correction process is initiated. First, based on the joint distribution and link shape of the robot arm, the robot arm body area is divided into multiple body partitions. In this embodiment, the area between the robot arm base and the first joint is divided into the first body partition, the link area between the first joint and the second joint is divided into the second body partition, and the link area between the second joint and the third joint is divided into the third body partition. The remaining links, wrist, and end effector body partitions are divided in the same way. The number of body partitions can be consistent with the number of joints of the robot arm, or they can be merged or subdivided according to the identifiability of each link in the depth image.

[0078] After the body is divided into sections, each section performs micro-displacement actions in a preset order. These micro-displacement actions are achieved by driving the corresponding joints to produce joint displacements smaller than the normal operating motion range. Different motion directions or execution sequences are set between each micro-displacement action, so that adjacent body sections form distinguishable positional changes in continuous depth images. For example, first, the joints near the robot's base are controlled to rotate a preset small angle along a first direction, then the middle joints are controlled to rotate a preset small angle along a second direction, and then the wrist joints are controlled to rotate a preset small angle along a third direction different from the second direction. After each micro-displacement action is completed, the robot returns to the starting state of the corresponding action or enters the starting pose of the next micro-displacement action.

[0079] During the micro-displacement movements of each body partition, the vision acquisition device continuously acquires images of the partition area according to a preset image acquisition cycle, while the joint state acquisition device synchronously acquires the joint pose data of the partition. The same time stamp is written to the partition area images and joint pose data at the same acquisition moment, thereby establishing a temporal correspondence between image changes and joint movements. Based on the robot's initial shape model and the current coordinate transformation relationship, the estimated projection area of ​​each body partition is determined in the partition area image, and a search range is set around the estimated projection area.

[0080] For each body partition, the depth point positions, contour positions, and depth changes within the contours in the continuous partition region images are compared. The set of depth points that change continuously with the corresponding micro-displacement action is extracted from the search range, and this set of depth points is determined as the actual image region of the corresponding body partition. The position changes of the actual image region are recorded according to the acquisition time sequence, and the actual image region at each time moment is converted into the actual position in the robot's base coordinate system, thereby generating the actual displacement trajectory of the corresponding body partition.

[0081] At the same time, based on the joint pose data of each body part collected at each moment and the kinematic model of the manipulator, the theoretical pose of each body part in the base coordinate system of the manipulator is determined. Then, according to the current coordinate transformation relationship, the theoretical position is transformed to the image space corresponding to the vision acquisition device, and the theoretical displacement trajectory of each body part is generated. The actual displacement trajectory reflects the actual motion result of the body part in the depth image, and the theoretical displacement trajectory reflects the motion result of the body part derived from the joint pose data under the current coordinate transformation relationship.

[0082] It should be noted that the kinematic model of the robot is pre-established based on the connection sequence of each joint of the robot, the geometric dimensions of each link, the motion type of each joint, and the positional relationship between each body partition and the corresponding joint, and stored in the safety controller. The robot base coordinate system is established with the robot base, and the coordinate systems of each joint are established in the order from the robot base to the end effector. The transformation relationship between adjacent joint coordinate systems is determined based on the link length, the direction of the joint axis, and the current joint position.

[0083] The structural parameters used in the kinematic model of the robot are obtained during robot installation or system initialization. These structural parameters include the length of each link, the position of each joint axis, the direction of each joint axis, the initial positional relationship between adjacent joints, the positional relationship between each body partition and the corresponding joint coordinate system, and the positional relationship of the end effector relative to the end joint. These structural parameters can be obtained from robot manufacturing parameters or calibrated using actual position data of the robot under multiple known joint poses.

[0084] When establishing the kinematic model of the robot using manufacturing parameters, the link dimensions, joint zero positions, and joint axis parameters provided by the robot manufacturer are written into the corresponding joint transformation unit. When establishing the kinematic model of the robot using calibration, the robot is controlled to move sequentially to multiple preset calibration poses, and the joint pose data and the actual position of the corresponding body partition under each calibration pose are collected. The structural parameters are adjusted according to the difference between the actual position and the model output position, and the adjusted structural parameters are used as the model parameters of the robot kinematic model.

[0085] The kinematic model of the robot includes multiple joint transformation units arranged in sequence. Each joint transformation unit corresponds to a joint of the robot. For a rotational joint, the input of the joint transformation unit is the rotational position of the corresponding joint; for a translating joint, the input of the joint transformation unit is the translating position of the corresponding joint. According to the joint arrangement order of the robot from the base to the end effector, the current joint pose data of each joint is read sequentially, and the current transformation relationship between adjacent joint coordinate systems is determined according to each joint transformation unit.

[0086] Starting from the robot's base coordinate system, the current transformation relationship between adjacent joint coordinate systems is superimposed sequentially according to the arrangement order of each joint, thereby determining the current theoretical pose of each joint, each link, and the end effector in the robot's base coordinate system. The theoretical pose includes the theoretical position and theoretical attitude of the corresponding body partition.

[0087] According to the execution sequence of micro-displacement actions, the actual displacement trajectory and theoretical displacement trajectory of each body partition are paired. During pairing, the starting position, direction of movement, sequence of positions passed through, and ending position of the trajectory are compared in sequence. Trajectory segments whose direction of movement is consistent with the theoretical displacement trajectory and whose sequence of positions passed through are corresponding are marked as normal trajectory segments; trajectory segments whose direction of movement is inconsistent, whose position change is interrupted, or whose sequence of positions passed through are not corresponding are marked as abnormal trajectory segments.

[0088] Since different body sections of the robot may occlude each other in the visual acquisition direction, some abnormal trajectory segments are not caused by coordinate transformation deviations, but by the image area of ​​the body section being occluded by adjacent body sections, end effectors, or the clamped workpiece. In order to distinguish between occluded trajectory segments and coordinate deviation trajectory segments, the abnormal trajectory segments are further judged according to the movement sequence of adjacent body sections.

[0089] When the actual displacement trajectory of a certain body partition is interrupted after an adjacent body partition enters its image projection range, and reappears after the adjacent body partition leaves, the abnormal trajectory segment corresponding to the interruption period is marked as an occluded trajectory segment. When the abnormal trajectory segment continues to exist with the complete micro-displacement action of the same body partition, and its offset direction does not change with the occlusion state of the adjacent partition, the abnormal trajectory segment is retained as the basis for judging coordinate deviation. The identified occluded trajectory segments are removed from the actual displacement trajectory, and the remaining actual displacement trajectory is compared with the theoretical displacement trajectory to generate the trajectory deviation sequence of each body partition.

[0090] In one optional implementation, when generating the trajectory deviation sequence of each body partition, the actual position of the i-th body partition at the k-th acquisition time is denoted as Pa(i,k), the corresponding theoretical position is denoted as Pt(i,k), and the difference between the two, Da(i,k)=Pa(i,k)-Pt(i,k), is determined as the trajectory deviation vector at that time. For the same body partition, multiple trajectory deviation vectors are arranged in the order of acquisition time to form the trajectory deviation sequence of the body partition.

[0091] Each trajectory deviation sequence records at least the deviation direction, deviation occurrence order, and deviation duration interval of the corresponding body partition. Multiple trajectory deviation sequences are sorted according to the distance between each body partition and the robot arm base, with body partitions closer to the robot arm base arranged first and those farther away arranged last. Common trajectory deviations with consistent directions that repeat in multiple body partitions are extracted from the multiple trajectory deviation sequences.

[0092] For example, when the actual displacement trajectories of the first to sixth body partitions are all offset in the same direction relative to the theoretical displacement trajectory, the directional deviation is determined as the common trajectory deviation. As another example, when the actual displacement trajectories of multiple body partitions change direction in the same order relative to the theoretical displacement trajectory, the order of the directional changes is taken as part of the common trajectory deviation. The common trajectory deviation is used to represent the overall offset of the visual acquisition device coordinate system relative to the robot's base coordinate system.

[0093] The current coordinate transformation relationship is adjusted based on the common trajectory deviation. During adjustment, the position of the origin and the direction of the coordinates in the current coordinate transformation relationship are changed sequentially. After each adjustment, the theoretical displacement trajectory of each body partition is regenerated. The regenerated theoretical displacement trajectory is compared with the actual displacement trajectory. When the trajectory deviation with the same direction in multiple body partitions is eliminated or transformed into local deviations that are inconsistent with each other, the current adjustment result is determined as the overall correction result.

[0094] After obtaining the overall correction results, the theoretical displacement trajectories of each body section are regenerated based on the adjusted coordinate transformation relationship, and the remaining trajectory deviations of each body section are re-determined. Local correction amounts are determined sequentially according to the distance between each body section and the robot arm base, from closest to furthest. For the body section closest to the robot arm base, the corresponding local correction amount is directly determined based on its remaining trajectory deviation. For subsequent body sections, the remaining trajectory deviation of the current body section is compared with the remaining trajectory deviation of the previous body section, and the newly added deviation between the two is determined as the local correction amount of the current body section.

[0095] After determining the local correction values ​​from near to far, each local correction value is verified in order from far to near. If the deviation direction and change sequence of a local correction value overlaps with the common trajectory deviation, the local correction value is discarded; if a local correction value only appears in the corresponding body partition and its subsequent partitions, the local correction value is retained. The retained local correction values ​​are merged with the overall correction result to obtain the corrected coordinate transformation relationship. The depth points in subsequent depth images are all transformed to the robot's base coordinate system based on the corrected coordinate transformation relationship.

[0096] Step S2: Based on the corrected coordinate transformation relationship, obtain the visible target occupied area in the working area from the depth image, and predict the distribution range of the visible target in the occupancy area according to the occlusion area formed by the robot body, workpiece or fixed facility and the historical motion state of the visible target, and generate the blind spot compensation occupied area.

[0097] After completing the coordinate transformation correction, the process of generating the visible target occupied area and the blind zone compensation occupied area begins. The vision acquisition device continuously acquires depth images of the robot's working area, and based on the corrected coordinate transformation relationship, transforms each depth point in the depth image to the robot's base coordinate system.

[0098] Based on the current joint pose and shape model of the robot, the spatial range corresponding to the robot body and end effector is determined; based on the current gripping state and workpiece model, the spatial range corresponding to the gripped workpiece is determined; based on the pre-stored fixed facility position data, the spatial range corresponding to the workpiece temporary storage rack, positioning stage and conveying device is determined, and depth points falling into the above spatial range are removed from the converted depth points to avoid identifying the robot body, workpiece or fixed facility as visible targets.

[0099] For the remaining depth points, clustering is performed according to spatial adjacency. Depth points that are spatially continuous and whose depth changes meet preset conditions are divided into the same target point set. The spatial unit occupied by each target point set is determined as the current occupied unit of the corresponding visible target, and all currently occupied units obtained at the same acquisition time are combined into the visible target occupied area.

[0100] Temporal correlation is performed on target point sets in continuous depth images. Specifically, the target point set in the current depth image is matched with the target point set in the previous depth image for proximity and contour continuity. When the position change direction of the two target point sets is continuous and the contour ranges correspond to each other, they are identified as the target point sets of the same visible target at different times. The same visible target is determined to be the same target if three conditions are met: ① Proximity: The spatial distance between the centroids of the target point sets in the previous and subsequent frames is ≤ 1.2 times the diameter of the maximum circumscribed sphere of the target point set; ② Contour continuity: The length, width and height deviation of the three-dimensional bounding box in the previous and subsequent frames are all ≤ 30%, and the contour overlap is ≥ 60%; ③ Directional continuity: The angle between the line connecting the centroids of the previous and subsequent frames and the motion direction at the previous moment is ≤ 45°. The spatial units occupied by the same visible target in sequence are recorded according to the acquisition time to generate the corresponding historical motion trajectory. The historical motion trajectory includes the order of spatial units traversed by the visible target, the order of changes in motion direction, and the time of entering or leaving each spatial region.

[0101] While determining the area occupied by the visible target, an occlusion area is generated. Based on the acquisition position of the vision acquisition device and the contours of the robot body, end effector, clamped workpiece, and fixed facilities in the depth image, the visible boundary of each entity facing the vision acquisition device and the occlusion boundary of the entity facing away from the vision acquisition device are determined. Starting from the acquisition position of the vision acquisition device, the occlusion path extends into the working area through the occlusion boundary.

[0102] For spatial units on the occlusion path, if the spatial unit has been identified as visible free space in the current depth image, it is removed from the occlusion path; if the spatial unit has been occupied by a fixed facility or the robot body, it is marked as a solid unit; the remaining spatial units that are not directly observed by the current depth image and are located behind the occlusion boundary are identified as blind space units in the occlusion area.

[0103] The obstructed area is divided according to the connectivity between blind zone spatial units. When multiple blind zone spatial units are adjacent through continuous boundaries, they are divided into the same blind zone unit. When the obstructed area branches at a certain location or is separated into different connectivity directions by fixed facilities, different blind zone units are established at the branching or separating locations. Each blind zone unit records its connectivity with adjacent blind zone units, its boundary relationship with the visible area, and its entrance relationship with the historical movement trajectory.

[0104] When a visible target moves from a visible area to an occluded area, the end position of its historical trajectory gradually approaches the boundary of the occluded area. The spatial unit at the end of the historical trajectory is matched with the entrance spatial unit of the adjacent visible area in each blind zone unit. When the spatial unit at the end of the historical trajectory is adjacent to an entrance spatial unit, and the final movement direction of the historical trajectory points to that entrance spatial unit, the blind zone unit to which that entrance spatial unit belongs is determined as the target blind zone unit, and a correlation is established between the historical trajectory and the target blind zone unit.

[0105] After determining the target blind zone unit, an entry direction sequence is generated based on the changes in the movement direction of the historical motion trajectory before entering the target blind zone unit. Specifically, multiple consecutive trajectory positions are selected forward from the end position of the historical motion trajectory. The directional changes between adjacent trajectory positions are recorded according to the chronological order of the trajectory positions. Five consecutive frames of trajectory points are selected forward from the end position of the historical motion trajectory, and the displacement direction vectors of adjacent frames are calculated in chronological order to form an entry direction sequence containing four direction vectors. If there are fewer than five valid trajectory points before entering the occlusion area, the direction vectors corresponding to all valid trajectory points are taken. The sequence length is not less than 2. For example, when the historical motion trajectory moves sequentially along the first direction, the second direction, and the third direction, the entry direction sequence records the first direction, the second direction, and the third direction sequentially. The entry direction sequence does not need to store the specific motion distance, but is used to represent the continuous directional change relationship of the visible target before entering the occlusion area.

[0106] Starting with the target blind zone unit, the path extends to adjacent blind zone units according to the entrance direction sequence. First, the adjacent blind zone units connected to the target blind zone unit are identified. Then, the spatial directions of each adjacent blind zone unit relative to the target blind zone unit are compared, and the adjacent blind zone unit corresponding to the current direction of the entrance direction sequence is selected. Subsequently, the selected adjacent blind zone unit is used as the new current unit, and the next blind zone unit is selected according to the next direction in the entrance direction sequence.

[0107] When a current blind zone unit connects to multiple adjacent blind zone units with similar directions, multiple extension branches are established, and the blind zone units passed through in sequence are recorded in each extension branch. When the entire entrance direction sequence has been used, but the current extension branch has not yet reached a blind zone unit adjacent to the visible area, the extension continues according to the last direction in the entrance direction sequence until it reaches the boundary of the occluded area, the boundary of the fixed facility, the area occupied by the robot body, or the end blind zone unit adjacent to the visible area. The blind zone units passed through in sequence in each extension branch form a forward unit chain.

[0108] Backtracking begins with the end blind zone cell adjacent to the visible area in each forward cell chain. Backtracking proceeds in the reverse order of the forward cell chains, sequentially selecting adjacent blind zone cells that can return to the previous blind zone cell, until the target blind zone cell is reached. When multiple adjacent blind zone cells exist during backtracking, the blind zone cell that also belongs to the corresponding forward cell chain is selected first. The blind zone cells traversed sequentially during backtracking form the reverse cell chain.

[0109] By comparing the forward and reverse unit chains corresponding to the same extended branch, the blind zone units that they both pass through are identified as candidate occupied units. Therefore, only blind zone units that simultaneously meet the conditions of forward extension from the target blind zone unit and reverse tracing from the adjacent end of the visible region are written into the candidate occupied unit set corresponding to that historical movement trajectory.

[0110] When multiple visible targets enter the same occlusion area or interconnected occlusion areas within the working area, candidate occupancy units are generated based on the historical movement trajectories of each visible target. These candidate occupancy units are then sorted according to the order in which they entered the occlusion area. For candidate occupancy units corresponding to the same historical movement trajectory, they are grouped into a candidate occupancy unit chain according to their order in the forward unit chain.

[0111] According to the sorting results, each candidate occupying unit chain is written into the occlusion area in sequence. The candidate occupying unit chain corresponding to the historical movement trajectory that first enters the occlusion area is written first. Subsequent candidate occupying unit chains are compared with the written candidate occupying unit chains in sequence. When the current candidate occupying unit chain does not overlap with the written candidate occupying unit chains, the current candidate occupying unit chain is directly written into the corresponding blind zone unit.

[0112] When the current candidate occupied cell chain overlaps with the already written candidate occupied cell chain, compare the direction and order of the overlapping cells in the two candidate occupied cell chains. If the direction of the overlapping cells in the two candidate occupied cell chains is the same, and the overlapping cells are arranged continuously in both candidate occupied cell chains, then retain the already written overlapping cells, and connect the non-overlapping cells in the current candidate occupied cell chain to the already written candidate occupied cell chain.

[0113] If the directions of the two candidate occupying unit chains through the overlapping units are not consistent, or if the overlapping units are not arranged continuously in one of the candidate occupying unit chains, then the overlapping unit corresponding to the historical movement trajectory that entered the occlusion area earlier is retained, and an unwritten blind zone unit is selected from the adjacent blind zone units of the overlapping unit. According to the extension direction of the historical movement trajectory that entered the occlusion area later, the corresponding overlapping unit is transferred to the selected unwritten blind zone unit, and the connection order between the candidate occupying units after the transfer is maintained.

[0114] After all candidate occupied cell chains have been written, the connectivity between each candidate occupied cell and its corresponding target blind zone cell entrance is checked. Candidate occupied cells that are no longer connected to their corresponding entrances due to overlapping transfers or path forks are deleted; candidate occupied cells that can still be connected to their corresponding entrances through consecutive blind zone cells are retained. The retained candidate occupied cells are merged to obtain the blind zone compensation occupied area.

[0115] The visible target occupied area represents the spatial range of the target that can be directly observed in the current depth image, while the blind zone compensation occupied area represents the occluded spatial range inferred from the historical motion trajectory and the connectivity of the occluded area. Occupation area type identifiers are set for the visible target occupied area and the blind zone compensation occupied area to distinguish the source of overlapping events in the subsequent risk assessment process.

[0116] Step S3: Based on the current pose, current speed, control delay, and braking parameters of the robot arm, determine the motion sweep area formed when the robot arm continues to move and the braking sweep area formed after receiving the stop command. Based on the overlap between the motion sweep area and the braking sweep area and the visible target occupied area and the blind zone compensation occupied area, determine the collision risk of the robot arm.

[0117] After obtaining the visible target occupied area and the blind zone compensation occupied area, a motion sweep unit chain and a braking sweep unit chain are constructed based on the current motion state of the robot arm. In this embodiment, the joint state acquisition device acquires the current joint position and current joint speed according to the robot arm control cycle, determines the current pose of the robot arm based on the current joint position, and reads the current motion commands that have not yet been completed in the robot arm controller.

[0118] Based on the current pose, current velocity, and current motion command, the pose changes of the robot arm in subsequent control cycles are predicted. For each predicted pose, based on the robot arm's shape model, the shape of the end effector, and the shape of the workpiece currently being held, the spatial units occupied by the robot arm as a whole in the corresponding control cycle are determined. The newly added, continuously occupied, and left spatial units of the robot arm in each control cycle are recorded in chronological order, and the spatial units passed through in consecutive control cycles are connected to form a motion sweep unit chain. The prediction cycle number is the smaller value between the remaining execution cycles of the current motion command and 20 control cycles. The control cycle is consistent with the robot arm's servo cycle and is set to 1ms. When the remaining execution cycles are greater than 20, only the pose and sweep space of the first 20 cycles are predicted. When the remaining cycles are less than 20, the prediction continues until the motion command is completed.

[0119] The motion sweep unit chain includes not only the motion path of the robot's end effector, but also the spatial units traversed by each robot body section and the workpiece being gripped as the current motion command proceeds. When different body sections pass through the same spatial unit within the same control cycle, the spatial unit is retained, and the earliest body section to enter that spatial unit and its corresponding motion sequence are recorded.

[0120] The braking initiation state of the robot is further determined based on the control delay. The control delay includes the processing delay required to complete the risk assessment, the communication delay for transmitting the stop command to the robot controller, and the response delay for the robot controller to start executing braking control after receiving the stop command. During the control delay, the robot continues to operate according to its current motion state. Starting from the current pose of the robot, the current speed is continued to advance along the current motion command until the end of the control delay, and the robot pose and joint speed corresponding to that moment are determined as the braking initiation state.

[0121] Read the braking parameters corresponding to each joint. The braking parameters include the deceleration sequence, stopping sequence, and joint position range before stopping when each joint starts braking from different speed states. Starting from the braking start state, according to the braking parameters of each joint, generate multiple braking poses of the manipulator from the start of braking to complete stop. The deceleration sequence refers to the order in which each joint starts braking, triggered in descending order of joint load. The stopping sequence refers to the order in which each joint's speed drops to zero. The joint position range refers to all intermediate angle intervals from the braking start angle to the stopping angle. The braking parameters are taken from the manipulator's factory calibration data or measured through pre-braking tests. Braking is triggered at 10%, 50%, and 100% of the rated speed, and the braking duration, angle change, and deceleration curve of each joint are recorded and stored in the braking parameter table.

[0122] For each braking pose, the spatial units occupied by each body section of the robot, the end effector, and the workpiece being gripped are determined, and the corresponding spatial units are connected according to the generation sequence of the braking pose to form a braking sweep unit chain. The end of the motion sweep unit chain is connected to the beginning of the braking sweep unit chain in chronological order. The motion sweep unit chain represents the spatial area traversed by the robot during the continued execution of the current motion command, and the braking sweep unit chain represents the spatial area traversed by the robot from the start of the stop command until the robot stops.

[0123] Subsequently, the visible target occupied area and the blind zone compensation occupied area are converted into an occupied unit sequence. For the visible target occupied area, a visible occupied order identifier is set for the corresponding spatial unit according to the passing order of each spatial unit in the historical movement trajectory of the visible target; for the blind zone compensation occupied area, a blind zone occupied order identifier is set for the corresponding spatial unit according to the order in which the candidate occupied unit chain extends from the entrance of the target blind zone unit into the interior of the occluded area. Spatial units with visible occupied order identifiers and blind zone occupied order identifiers form an occupied unit sequence according to the target association relationship.

[0124] According to the arrangement order of the spatial units in the motion sweep unit chain, braking sweep unit chain, and occupying unit sequence, motion sequence identifiers, braking sequence identifiers, and occupying sequence identifiers are set for each spatial unit. The sequence identifiers are not limited to continuous values, but can also use sequential information corresponding to the control cycle, trajectory nodes, or spatial unit arrangement positions.

[0125] Following the arrangement order of the motion sweep unit chain, starting from the beginning of the motion sweep unit chain, spatial units within it are sequentially matched with spatial units in the occupied unit sequence. When a spatial unit in the motion sweep unit chain corresponds to the same spatial range as a spatial unit in the occupied unit sequence, or when their corresponding spatial ranges overlap, the corresponding occupied spatial unit is recorded as the first matching unit, along with its motion order identifier, occupied order identifier, and occupied region type.

[0126] When the same motion space unit overlaps with multiple occupied space units, multiple first matching units are recorded according to the order of arrangement in the occupied unit sequence; when the same occupied space unit overlaps with multiple motion space units, the motion order corresponding to each motion space unit is retained, and the earliest time when the robot enters the occupied space unit is determined.

[0127] After completing the forward matching of the motion sweep unit chain and the occupied unit sequence, braking space units are selected sequentially from the end of the braking sweep unit chain according to the reverse arrangement order of the braking sweep unit chain. Each braking space unit is matched with the first matching unit. When a first matching unit coincides with both the space units in the motion sweep unit chain and the space units in the braking sweep unit chain, the first matching unit is determined as the second matching unit.

[0128] Reverse matching begins at the stop end of the braking sweep unit chain, recording the order in which the occupying space units appear in the rear, middle, and front sections of the manipulator's braking. For the first matching unit that only overlaps with the motion sweep unit chain and not the braking sweep unit chain, its motion sequence identifier is retained, and it is marked as a motion phase matching unit; for the second matching unit that overlaps with both the motion sweep unit chain and the braking sweep unit chain, both the motion sequence identifier and the braking sequence identifier are recorded.

[0129] The second matching unit and the retained motion phase matching units are sorted according to the motion sequence identifier. The corresponding spatial units are determined as overlapping spatial units, and the motion sequence corresponding to the first-sorted matching unit is determined as the first overlapping sequence. When there are no motion phase matching units but only overlap between the braking sweep unit chain and the occupying unit sequence, the first overlapping sequence is determined according to the forward execution order of the braking sweep unit chain.

[0130] After determining the overlapping spatial units and the initial overlap order, a stage identifier is set for each overlapping spatial unit based on its position within the motion sweep unit chain or the braking sweep unit chain. Overlapping spatial units located in the motion sweep unit chain and earlier than the braking start state are set as motion stage identifiers; overlapping spatial units located at the connection point between the motion sweep unit chain and the braking sweep unit chain are set as braking start stage identifiers; overlapping spatial units located in the braking sweep unit chain are set as braking stage identifiers. Simultaneously, the initial overlap order and occupied region type are recorded for each overlapping spatial unit.

[0131] The overlapping spatial units are arranged according to the order of their first overlap, and overlapping spatial units that are consecutive in position, have the same stage identifier, and occupy the same type of area are merged into one overlapping event segment. For example, if multiple consecutive spatial units are all located in the motion sweep unit chain and all correspond to the area occupied by the visible target, they are merged into one visible motion overlapping event segment; if multiple consecutive spatial units are all located in the braking sweep unit chain and all correspond to the area occupied by blind zone compensation, they are merged into one blind zone braking overlapping event segment.

[0132] Starting with the overlapping event segment whose initial overlap sequence is first, the phase changes and occupied region type changes between each overlapping event segment are determined sequentially along the arrangement direction of the motion sweep unit chain and the braking sweep unit chain, thus forming a collision transfer sequence. The collision transfer sequence records the order in which overlapping events extend from the motion phase to the braking phase, and the order in which the corresponding overlapping region changes from the visible target occupied region to the blind zone compensation occupied region or from the blind zone compensation occupied region to the visible target occupied region.

[0133] When the first overlapping event segment is located in the motion sweep unit chain and the collision transmission sequence does not extend to the braking sweep unit chain, the collision risk of the manipulator is identified as a motion intrusion risk. This risk indicates that during the continued execution of the current motion command, the manipulator's motion sweep area overlaps with the visible target occupied area or the blind zone compensation occupied area.

[0134] When the first overlapping event segment is located in the motion sweep unit chain and the collision transmission sequence continues to extend to the braking sweep unit chain, or when the first overlapping event segment is located directly in the braking start position or in the braking sweep unit chain, the collision risk of the manipulator is identified as the braking continuation risk. This risk means that even if the manipulator receives a stop command, it will still pass through the corresponding overlapping space unit during the control delay and braking period.

[0135] When the first overlapping event segment corresponds to the blind zone compensation area, and there is no preceding overlapping event segment in the collision transmission sequence that corresponds to the visible target area, the collision risk of the manipulator is determined as a blind zone undetermined risk. This risk indicates that the overlapping spatial unit is derived from the historical trajectory extrapolation of the occluded area, and the corresponding target has not yet been directly observed in the current depth image.

[0136] Step S4: When the collision risk reaches the stop threshold, control the robot arm to stop moving. When the collision risk is caused by the blind spot compensation area and has not reached the stop threshold, control the robot arm to move along the preset safety trajectory and re-perform visual detection. After the collision risk is lower than the safety threshold, allow the robot arm to continue performing the operation.

[0137] Based on the collision risk type, the initial overlap order, the position of the overlapping event segment in the motion sweep unit chain or braking sweep unit chain, and the continuous length of the overlapping event segment, it is determined whether the current collision risk has reached the stopping threshold. The stopping threshold is set separately for different collision risk types: Stopping is triggered when the initial overlap order of motion intrusion risk is ≤5 control cycles, and the initial overlap order of braking continuation risk is ≤10 control cycles; Safety threshold: No overlap throughout the motion and braking sweep unit chains, or an initial overlap order ≥30 control cycles; The restart threshold is the same as the safety threshold. The safety trajectory is preset to execute 3 times; if the blind zone risk is still not eliminated after 3 consecutive executions, stopping is triggered.

[0138] After receiving the stop command, the robot controller controls each joint to stop moving according to the preset braking method. During the robot's stop, depth images and joint pose data are continuously acquired, and the visible target occupied area, blind spot compensation occupied area, and braking sweep unit chain are updated. When the robot has completely stopped, the visual detection state is maintained until the collision risk is lower than the restart threshold, and then a decision is made on whether to resume the original operation trajectory according to the operation control strategy.

[0139] When a collision risk is caused by a blind spot compensation area, is identified as a blind spot pending risk, and has not reached the stopping threshold, the robot is not allowed to continue running according to the original motion command. Instead, a preset safety trajectory corresponding to the current robot pose is selected from multiple preset safety trajectories. The preset safety trajectory is a small-range observation trajectory executed by the robot near the current working position, and each trajectory node is located within the space range that has been confirmed to be unoccupied by a visible target.

[0140] Preset safety trajectory generation rules: A candidate safety trajectory library is generated for common robot arm working postures. The generation rules are as follows: Each trajectory starts from the corresponding posture, the total displacement does not exceed one-third of the normal single-step displacement at that position, and the movement speed is set to 20% of the normal working speed; the spatial units traversed by the trajectory are all confirmed visible free space units, and there is no spatial overlap with fixed facilities or known workpiece positions; each type of reference posture corresponds to 3 types of trajectories: wrist deflection type, middle link retraction type, and end effector reverse movement type, each type includes trajectories with three displacement amplitudes: large, medium, and small.

[0141] Multiple candidate safety trajectories are pre-set for different robot arm poses, such as a first safety trajectory that causes the robot wrist to deflect in a first direction, a second safety trajectory that causes the middle link of the robot arm to retract in a second direction, and a third safety trajectory that causes the end effector to move in the opposite direction of the current working trajectory. The candidate sweep area corresponding to each candidate safety trajectory is determined, and candidate safety trajectories that overlap with the area occupied by the currently visible target are eliminated.

[0142] For the remaining candidate safety trajectories, based on the new occlusion boundary formed after the robot arm executes the candidate safety trajectory, the corresponding occlusion area is re-deduced, and the number of blind zone units, the entry position of the target blind zone unit, and the candidate occupied unit chain are compared before and after the execution of the candidate safety trajectory. The candidate safety trajectory that causes the current blind zone compensation occupied area to change position or shrink in range after execution, and the candidate sweep area does not pass through the current overlapping spatial unit, is selected as the preset safety trajectory for this time.

[0143] The robot arm is controlled to move along the selected preset safety trajectory at a preset safety speed lower than the normal operating speed. During the execution of the safety trajectory, the vision acquisition device continuously acquires depth images, and the joint state acquisition device continuously acquires joint pose data. Based on the new robot arm pose, the occupancy boundary formed by the robot arm body, workpiece and fixed facility is re-determined, and the visible target occupied area and blind spot compensation occupied area are regenerated.

[0144] When a spatial cell in the original blind zone compensation area becomes visible free space in the new depth image, the corresponding spatial cell is deleted from the blind zone compensation area. When a target depth point is detected in a spatial cell in the original blind zone compensation area, the corresponding spatial cell is transferred to the visible target area, and its historical motion trajectory is updated according to the newly obtained visible target position. When some spatial cells are still occluded, candidate occupants are regenerated according to the updated occupancy boundary, historical motion trajectory, and blind zone cell connectivity.

[0145] After completing the re-visual inspection, based on the robot's new current pose, new current speed, and the original work instructions that have not yet been completed, the motion sweep unit chain and braking sweep unit chain are regenerated and re-matched with the updated occupancy unit sequence. If the reassessed collision risk is below the safety threshold, the robot is allowed to return to the original work trajectory from the current safe trajectory endpoint and continue performing the workpiece handling operation.

[0146] If the re-determined collision risk is still a blind zone pending risk, but the corresponding first overlap order is shifted, another preset safe trajectory can be selected from the remaining candidate safe trajectories, and the visual detection can be repeated. If the blind zone pending risk is still not lower than the safety threshold after the preset number of safe trajectory executions, or if a motion intrusion risk or braking continuation risk occurs during the re-detection process, a stop command is sent to the robot controller.

[0147] As a specific operation process in this embodiment, after the robot arm picks up a workpiece from the conveying device, it moves towards the positioning table along the preset transport trajectory. When the robot arm enters the middle section of the transport trajectory, the vision acquisition device continuously acquires depth images and identifies a movable turnover box moving along the outer channel of the worktable. The turnover box forms a visible target occupied area in the continuous depth images, records the spatial units it passes through in sequence, and generates a historical motion trajectory.

[0148] As the robotic wrist and the clamped workpiece rotate, the turnover box is gradually obscured by the clamped workpiece in the acquisition direction of the vision acquisition device. Based on the end position of the turnover box's historical movement trajectory, it is matched with the entrance position of the obscured area behind the clamped workpiece to determine the corresponding target blind zone unit. An entrance direction sequence is generated based on the change in the movement direction of the turnover box before entering the obscured area. Forward path extension and reverse path backtracking are performed between adjacent blind zone units in the obscured area to generate candidate occupying units.

[0149] At this time, another movable turnover box in the working area has entered the same occupancy area from another entrance, and a first candidate occupying unit chain has been generated for it. According to the order in which the two historical movement trajectories entered the occupancy area, the first candidate occupying unit chain is written first, and then the current candidate occupying unit chain is written. When the two candidate occupying unit chains coincide at the bifurcation position in the occupancy area, the directions of the two passing through the overlapping units are compared. Since the two directions are different, the overlapping units in the first candidate occupying unit chain that entered the occupancy area first are retained, and the current candidate occupying unit chain that entered the occupancy area later is transferred to the adjacent unwritten blind zone unit. After completing the connectivity check, the space units retained in the two candidate occupying unit chains are merged into the blind zone compensation occupying area.

[0150] A motion sweep unit chain is generated based on the current pose, current speed and current handling command of the robot arm. Then, a braking sweep unit chain is generated based on the control delay and braking parameters of each joint. The motion sweep unit chain and the braking sweep unit chain are matched with the occupancy unit sequence formed by the visible target occupancy area and the blind zone compensation occupancy area, respectively.

[0151] The matching results show that when the robot continues to execute the current handling command, some motion sweep units corresponding to the end effector overlap with some occupied units in the blind zone compensation area. However, this overlap event does not extend to the current braking sweep unit chain, and the first overlap order is located at the end of the motion sweep unit chain. This collision risk is identified as a blind zone pending risk, and it is determined that it has not reached the stopping threshold.

[0152] A safe trajectory is selected from the candidate safe trajectories to deflect the robotic wrist away from the obstructed area. The robotic wrist is controlled to execute the safe trajectory at a preset safe speed. After the robotic wrist deflects, the obstruction boundary originally formed by the clamped workpiece moves. The vision acquisition device re-observes some of the original blind zone spatial units. Based on the new depth image, some of these spatial units are determined to be free space and are deleted from the blind zone compensation area. At the same time, the depth point corresponding to the turnover box is re-detected in another part of the spatial units, and this part of the spatial units is moved into the visible target occupied area.

[0153] The motion sweep unit chain and braking sweep unit chain of the robot arm when returning from the current pose to the original transport trajectory are regenerated and matched with the updated occupancy unit sequence again. After rematching, neither the motion sweep unit chain nor the braking sweep unit chain overlaps with the visible target occupancy area or the remaining blind zone compensation occupancy area. The collision risk is determined to be lower than the safety threshold, and the robot arm is allowed to return to the original transport trajectory and continue to move the workpiece to the positioning stage.

[0154] During the same operation, when the visual acquisition device experiences a slight positional change due to the vibration of the worktable, the robot arm is controlled to perform micro-displacement actions on each body section in the next operation interval. It is found that the actual displacement trajectories of multiple body sections are all offset in the same direction relative to the theoretical displacement trajectory. Therefore, the common trajectory deviation is extracted, and the current coordinate transformation relationship is adjusted as a whole. For the remaining trajectory deviations that still exist in the wrist body section, the local correction amount is determined in order from near to far. During the verification process from far to near, the local correction amount corresponding only to the wrist body section is retained. Thus, a new coordinate transformation relationship is obtained. The subsequent depth point transformation, occlusion area division, and sweep area matching all adopt the new coordinate transformation relationship.

[0155] Through the above process, in the same safety detection cycle, this embodiment sequentially links the coordinate relationship correction between the visual acquisition device and the robot arm, the extraction of the visible target occupied area, the deduction of the target position in the occluded area, the prediction of the robot arm's continued movement range, the control delay compensation, the prediction of the braking movement range, and the collision risk classification. The data formed in the previous processing step is used as the input for the next processing step, and is updated again after the robot arm performs a stop action or a safety trajectory. Safety detection is continuously performed based on the current visual state, historical movement state, and the actual movement state of the robot arm.

[0156] It should be noted that the number of manipulator joints, the number of body partitions, the size of spatial units, the sequence of micro-displacement actions, the depth image acquisition cycle, the control delay, the number of candidate safe trajectories, and the risk threshold in this embodiment can all be set according to the manipulator model, the performance of the vision acquisition device, and the size of the working area. The manipulator can be a four-axis manipulator, a seven-axis manipulator, a parallel manipulator, or other actuators that can determine the body position based on joint pose data. The vision acquisition device can be a single depth camera or multiple depth cameras with overlapping acquisition areas. When multiple vision acquisition devices are used, the coordinate transformation relationship between each vision acquisition device and the manipulator is corrected separately, and the visible target occupied area obtained by each vision acquisition device is converted to the same manipulator base coordinate system before being merged.

[0157] The visible target referred to in this embodiment is not limited to a specific category. As long as a set of target points and historical motion trajectories can be formed based on continuous depth images, the visible target occupied area and blind spot compensation occupied area can be generated according to the above method. The fixed facilities are not limited to workpiece temporary storage racks, positioning tables and conveying devices, but also include protective columns, processing equipment, material supports, tool cabinets or other relatively fixed entities. The preset safety trajectory can be generated based on the work area model before the robot is put into operation, or it can be generated after selecting a continuous spatial path from the currently confirmed idle spatial units when a blind spot risk is detected.

[0158] The execution order of the steps described in this embodiment is used to illustrate a continuous implementation method. Without changing the data correlation between coordinate correction, blind spot compensation, sweep area generation, and risk assessment, some data acquisition and processing steps can be executed in parallel. For example, while generating the visible target-occupied area of ​​the current frame, the motion sweep unit chain of the manipulator can be updated based on the joint pose data of the previous frame; during the manipulator's execution of a safe trajectory, the occlusion area and braking sweep unit chain can be updated synchronously. The above parallel processing still falls under the implementation method of this embodiment.

[0159] Example 2

[0160] Please see Figure 4Another embodiment of the present invention provides a vision-based robotic arm safety detection system, comprising: a coordinate correction module, a blind spot compensation module, a risk assessment module, and a safety control module;

[0161] The coordinate correction module is used to acquire depth images and joint pose data of the working area of ​​the robot arm, identify the robot arm body area, and correct the coordinate transformation relationship between the vision acquisition device and the robot arm based on the deviation between the actual position and the theoretical position of the robot arm body area.

[0162] The blind spot compensation module is used to obtain the visible target occupied area in the working area from the depth image based on the corrected coordinate transformation relationship, and predict the distribution range of the visible target in the occupancy area according to the occupancy area formed by the robot body, workpiece or fixed facility and the historical motion state of the visible target, and generate the blind spot compensation occupied area.

[0163] The risk assessment module is used to determine the motion sweep area formed when the robot continues to move and the braking sweep area formed after receiving the stop command, based on the robot's current pose, current speed, control delay and braking parameters. It also determines the collision risk of the robot based on the overlap between the motion sweep area and the braking sweep area and the visible target occupied area and the blind zone compensation occupied area.

[0164] The safety control module is used to control the robot to stop moving when the collision risk reaches the stop threshold, and to control the robot to move along a preset safety trajectory and re-perform visual detection when the collision risk is caused by the blind spot compensation area and has not reached the stop threshold. After the collision risk is lower than the safety threshold, the robot is allowed to continue to perform the operation.

[0165] In addition, the parts of the technical solutions provided in the embodiments of this application that are consistent with the implementation principles of the corresponding technical solutions in the prior art have not been described in detail, so as to avoid excessive elaboration.

[0166] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the invention. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A vision-based safety inspection method for robotic arms, applied to a robotic arm operating system including a robotic arm, a vision acquisition device, a joint state acquisition device, and a safety controller, characterized in that, include: The system acquires depth images and joint pose data of the robot's working area, identifies the robot's body area, and corrects the coordinate transformation relationship between the vision acquisition device and the robot based on the deviation between the actual and theoretical positions of the robot's body area. Based on the corrected coordinate transformation relationship, the visible target occupied area in the working area is obtained from the depth image. Based on the occupancy area formed by the robot body, workpiece or fixed facility and the historical motion state of the visible target, the distribution range of the visible target in the occupancy area is predicted, and the blind spot compensation occupied area is generated. Based on the current pose, current speed, control delay, and braking parameters of the robotic arm, determine the motion sweep area formed when the robotic arm continues to move and the braking sweep area formed after receiving the stop command. Based on the overlap between the motion sweep area and the braking sweep area and the area occupied by the visible target and the blind zone compensation area, determine the collision risk of the robotic arm. When the collision risk reaches the stop threshold, the robot arm is controlled to stop moving. When the collision risk is caused by the blind spot compensation area and has not reached the stop threshold, the robot arm is controlled to move along the preset safety trajectory and perform visual detection again. Once the collision risk is lower than the safety threshold, the robot arm is allowed to continue performing the operation.

2. The vision-based robotic arm safety detection method as described in claim 1, characterized in that, The step of correcting the coordinate transformation relationship between the vision acquisition device and the robot arm based on the deviation between the actual position and the theoretical position of the robot arm body area includes: The robot body area is divided into multiple body partitions, and each body partition is controlled to perform micro-displacement movements in a preset order, while simultaneously acquiring images of the partition area and the joint pose data of the partition. The actual displacement trajectory of each body partition is determined based on the partitioned region image, and the corresponding theoretical displacement trajectory is determined based on the partitioned joint pose data and the manipulator kinematic model. Based on the execution sequence of micro-displacement actions, the actual displacement trajectory is paired with the theoretical displacement trajectory to identify abnormal trajectory segments that do not match. Based on the motion sequence of adjacent body partitions, identify and remove trajectory segments caused by occlusion from the abnormal trajectory segments, and generate a trajectory deviation sequence for each body partition based on the deviation between the remaining actual displacement trajectory and the theoretical displacement trajectory. Based on the trajectory deviation sequence, the coordinate deviations common to each body partition are first corrected, and then the corresponding local coordinate deviations are corrected in the order of the distances between each body partition and the robot arm base to obtain the corrected coordinate transformation relationship.

3. The vision-based robotic arm safety detection method as described in claim 2, characterized in that, Based on the trajectory deviation sequence, the common coordinate deviations of each body section are first corrected, and then the corresponding local coordinate deviations are corrected in order of distance between each body section and the robot arm base to obtain the corrected coordinate transformation relationship, including: The trajectory deviation sequence is sorted according to the distance between each body partition and the robot arm base, and common trajectory deviations with consistent direction are extracted from multiple body partitions; The current coordinate transformation relationship is adjusted according to the common trajectory deviation, and the theoretical displacement trajectory of each body partition is regenerated based on the adjusted coordinate transformation relationship. Based on the regenerated theoretical displacement trajectory, the remaining trajectory deviation of each body section is determined, and the corresponding local correction amount is determined sequentially according to the distance between each body section and the robot arm base from near to far. The local correction values ​​are verified in order from farthest to nearest. Local correction values ​​that are duplicated with the common trajectory deviation are removed. The retained local correction values ​​are then merged with the adjusted coordinate transformation relationship to obtain the corrected coordinate transformation relationship.

4. The vision-based robotic arm safety detection method as described in claim 1, characterized in that, The blind spot compensation area includes: Based on the corrected coordinate transformation relationship, the depth image is transformed to the robot's base coordinate system, the depth points corresponding to the robot body, workpiece and fixed facilities are removed, the area occupied by the visible target is obtained, and the historical motion trajectory of the visible target is generated based on the continuous depth image. Based on the obstruction boundary formed by the robot body, workpiece, and fixed facilities, the obstruction area extending from the obstruction boundary into the working area is determined, and the obstruction area is divided into multiple blind zone units according to the spatial connectivity. The end position of the historical motion trajectory is matched with the entrance position of each blind spot unit to determine the target blind spot unit associated with the historical motion trajectory; Based on the direction and sequence of movement of the historical trajectory, the path is extended from the entrance position of the target blind zone unit to the adjacent blind zone unit, and the blind zone unit adjacent to the visible area is traced back to the entrance position. The blind zone unit whose path extension result and the reverse tracing result coincide is determined as the candidate occupied unit. According to the order in which each historical movement trajectory enters the occupied area, the corresponding candidate occupied units are merged, and the merged candidate occupied units are determined as the blind zone compensation occupied area.

5. The vision-based robotic arm safety detection method as described in claim 4, characterized in that, The process of identifying blind zone units where the path extension results coincide with the reverse backtracking results as candidate occupied units includes: Based on the changes in the direction of motion before entering the target blind zone unit according to the historical motion trajectory, an entry direction sequence is generated; Starting with the target blind zone unit, the system extends to adjacent blind zone units according to the entrance direction sequence, and records the blind zone units passed through in sequence to form a forward unit chain. Starting from the end blind zone unit adjacent to the visible area in the forward unit chain, backtracking is performed towards the target blind zone unit in the reverse order of the forward unit chain to form a reverse unit chain; The blind zone cells that the forward cell chain and the reverse cell chain both pass through are identified as candidate occupied cells.

6. The vision-based robotic arm safety detection method as described in claim 5, characterized in that, The step of merging the corresponding candidate occupied units according to the order in which each historical movement trajectory enters the occupied area, and determining the merged candidate occupied units as the blind spot compensation occupied area, includes: According to the order in which each historical movement trajectory enters the occupied area, the corresponding candidate occupied units are sorted to form multiple candidate occupied unit chains; According to the sorting results, each candidate occupying unit chain is written into the occluded area in sequence; When the current candidate occupied cell chain overlaps with the written candidate occupied cell chain, non-overlapping cells with the same direction and continuous arrangement are connected to the written candidate occupied cell chain, and overlapping cells with inconsistent direction or discontinuous arrangement are transferred to adjacent unwritten blind zone cells. Delete candidate occupied units that are not connected to the entrance of the corresponding target blind zone unit, and merge the remaining candidate occupied units to obtain the blind zone compensation occupied area.

7. The vision-based robotic arm safety detection method as described in claim 1, characterized in that, The process involves determining the motion sweep area formed when the robot continues to move and the braking sweep area formed after receiving a stop command, based on the robot's current pose, current speed, control delay, and braking parameters. It also involves determining the robot's collision risk based on the overlap between the motion sweep area and the braking sweep area and the visible target-occupied area and the blind spot compensation area. Based on the current pose, current speed, control delay, and braking parameters of the robot arm, the spatial units that the robot arm will pass through in sequence as it continues to move are determined, forming a chain of motion sweeping units; The braking start state of the robot is determined based on the control delay, and the spatial units that the robot passes through in sequence before stopping are determined based on the braking parameters, forming a braking sweep unit chain connected to the motion sweep unit chain. According to the historical movement sequence of visible targets and the extension sequence of candidate occupied units, the spatial units in the occupied area of ​​visible targets and the occupied area of ​​blind zone compensation are sorted to form an occupied unit sequence; The motion sweep unit chain and the braking sweep unit chain are matched with the occupied unit sequence to determine the overlapping spatial units and their first overlap order. The collision risk of the robot is determined based on the position of the overlapping spatial unit in the motion sweep unit chain or braking sweep unit chain, the first overlap order, and the corresponding occupied area type.

8. The vision-based robotic arm safety detection method as described in claim 7, characterized in that, The step of matching the motion sweep unit chain and the braking sweep unit chain with the occupied unit sequence to determine the overlapping spatial units and their first overlap order includes: Each spatial unit is assigned a corresponding sequence identifier according to its arrangement in the motion sweep unit chain, braking sweep unit chain, and occupied unit sequence. According to the arrangement order of the motion sweep unit chain, the spatial units are matched with the occupied unit sequence, and the first matching unit that overlaps is recorded. According to the reverse arrangement order of the braking sweep unit chain, the spatial units therein are matched with the first matching unit to determine the second matching unit that coincides with both the motion sweep unit chain and the braking sweep unit chain. The second matching units are sorted according to the motion sequence identifier, the corresponding spatial units are determined as overlapping spatial units, and the motion sequence of the first sorted second matching unit is determined as the first overlapping sequence.

9. The vision-based robotic arm safety detection method as described in claim 8, characterized in that, Based on the position of the overlapping spatial unit in the motion sweep unit chain or braking sweep unit chain, the order of the first overlap, and the corresponding occupied area type, the collision risk of the robot arm is determined, including: Based on the position of each overlapping spatial unit in the motion sweep unit chain or braking sweep unit chain, set the corresponding stage identifier and record the first overlap order and occupied area type. Arrange the overlapping spatial units according to the first overlapping order, and merge the overlapping spatial units that are consecutive in position, have the same stage identifier and occupy the same area type into overlapping event segments. Along the arrangement direction of the motion sweep unit chain and the braking sweep unit chain, the stage changes and occupied area type changes of each overlapping event segment are determined sequentially to form a collision transmission sequence. Based on the stage identifier corresponding to the first overlapping event segment, whether the collision transmission sequence extends to the braking sweep unit chain, and the corresponding occupied area type, the collision risk of the manipulator is determined as motion intrusion risk, braking continuation risk, or blind zone undetermined risk.

10. A vision-based robotic arm safety inspection system, used to implement the vision-based robotic arm safety inspection method according to any one of claims 1-9, characterized in that, include: Coordinate correction module, blind spot compensation module, risk assessment module, and safety control module; The coordinate correction module is used to acquire depth images and joint pose data of the working area of ​​the robot arm, identify the robot arm body area, and correct the coordinate transformation relationship between the vision acquisition device and the robot arm based on the deviation between the actual position and the theoretical position of the robot arm body area. The blind spot compensation module is used to obtain the visible target occupied area in the working area from the depth image based on the corrected coordinate transformation relationship, and predict the distribution range of the visible target in the occupancy area according to the occupancy area formed by the robot body, workpiece or fixed facility and the historical motion state of the visible target, and generate the blind spot compensation occupied area. The risk assessment module is used to determine the motion sweep area formed when the robot continues to move and the braking sweep area formed after receiving the stop command, based on the robot's current pose, current speed, control delay and braking parameters. It also determines the collision risk of the robot based on the overlap between the motion sweep area and the braking sweep area and the visible target occupied area and the blind zone compensation occupied area. The safety control module is used to control the robot to stop moving when the collision risk reaches the stop threshold, and to control the robot to move along a preset safety trajectory and re-perform visual detection when the collision risk is caused by the blind spot compensation area and has not reached the stop threshold. After the collision risk is lower than the safety threshold, the robot is allowed to continue to perform the operation.

Images