Anti-dynamic occlusion mechanical arm visual servo method and system

Abstract

The present application relates to the fields of robot technology and computer vision technology, and particularly relates to a dynamic occlusion-resistant robot arm visual servo method and system, wherein a light response reference model is calibrated before servo; in the servo process, an environmental monitoring system senses dynamic obstacles and predicts their movement; according to the predicted light disturbance, an active lighting array is reversely compensated to maintain image quality; the system continuously evaluates a comprehensive stability index, and performs hierarchical responses according to the index value, including light compensation, path maintenance or obstacle avoidance re-planning. The present application solves the technical problem that the prior art fails to effectively handle light mutation and physical shielding when dynamic occlusion occurs, resulting in early failure of the visual system, and provides a dynamic occlusion-resistant robot arm visual servo method and system, which can predict and actively compensate for light disturbance, ensure servo task continuity under a multi-level response mechanism, and guarantee the stability and robustness of the visual system in a complex dynamic environment.

Description

Technical Field

[0001] This invention relates to the fields of robotics and computer vision, and in particular to a visual servoing method and system for a robotic arm that resists dynamic occlusion. Background Technology

[0002] With the deep integration of intelligent manufacturing and automation technologies, robots with robotic arms as their core component are widely used in critical processes such as precision assembly, robotic arm grasping, and surgery. Visual servoing, as a key technology that endows robotic arms with the ability to perceive and control their environment, uses closed-loop control to drive the end effector of the robotic arm to continuously approach and manipulate the target in real time using visual information, thereby greatly improving the intelligence level and operational accuracy of the entire system. Therefore, the robustness and stability of visual servoing systems, especially their adaptability to complex unstructured environments, are crucial to the overall performance and application level of robotic arm systems.

[0003] To address the prevalent problem of target occlusion in practical operations, many technologies have been explored and tested. For example, Chinese patent CN114793631B discloses a harvesting end effector based on visual guidance and dynamic control. In harvesting operations, overlapping branches and leaves or mutual occlusion of fruits frequently occur. This technical solution, using a depth camera and precise control algorithms, can solve the occlusion problem caused by overlapping branches or fruits in agricultural harvesting. By acquiring 3D point cloud data of the target through a depth camera, even if some features of the target are incomplete, the target's pose can be reconstructed through model matching or geometric analysis, thereby guiding the harvesting end effector to grasp the target. This essentially solves the occlusion problem in static or quasi-static harvesting operations. This solution represents the development level of this technology in a specific scenario. The concept of this solution is to treat occlusion as a predetermined, static event based on geometric relationships, and passively respond to the occurrence of occlusion by enhancing the analysis capability of residual information.

[0004] Therefore, when robot vision servo systems are applied to new application scenarios that shift from single-structure, static environments to dynamic human-robot collaboration and complex equipment environments, new and higher requirements are placed on system robustness and real-time performance. In these new application scenarios, occlusion is no longer a static geometric problem, but a dynamic process, usually caused by moving obstructions within the workspace (e.g., components of a conveyor belt, other equipment involved in the operation, or even the human body). The existing technical solutions have certain limitations in their guiding principles. The main contradiction lies in the fact that these solutions separate the physical phenomenon of occlusion from the optical problem of the inevitable abrupt change in the lighting environment, resulting in a passive and lagging outcome.

[0005] This is because when a dynamic occluder gradually approaches and eventually occludes a visual servoing target, the visual servoing system is affected by the occlusion not only when physical occlusion occurs, but also before that. The moving occluder causes continuous, drastic, and unpredictable dynamic changes in the illumination field around the visual servoing target. For example, the movement of the occluder can create rapidly moving shadows on the target surface, thus changing the brightness and contrast of the target surface; if the occluder has reflective properties, it may also create highlights or mirror reflections near the target. These illumination perturbations are fatal to visual servoing algorithms that rely on lower-level features such as image grayscale, gradients, and textures. Whether it's traditional feature-point-based matching methods (such as SIFT and SURF) or deep learning-based feature extraction networks, their stable operation depends on relatively uniform and consistent illumination conditions. Dramatic changes in illumination can directly contaminate or destroy the features of the target, leading to feature extraction failures or matching errors. Corresponding visual servoing systems may fail prematurely due to incorrect pose estimation, even when the target is clearly visible and there are no physical obstructions, failing to stably track features and interrupting servoing. This perception failure, caused by illumination interference as a precursor, occurs earlier than physical occlusion, significantly reducing the system's effective working window and operational smoothness in dynamic environments. Because existing technologies lack a continuous and forward-looking understanding and response to the occlusion-illumination coupling process, their vulnerability to dynamic threats fails to meet the demands of highly dynamic and reliable applications. The new technical problem faced by those skilled in the art has evolved from simply solving the passive problem of how to recover the target after occlusion to how to proactively and collaboratively anticipate and intervene in illumination changes and physical occlusion throughout the entire dynamic occlusion process. Therefore, how to provide a visual servoing control method that treats dynamic occlusion as a continuous process including illumination disturbance precursors and physical occlusion results, and based on this, performs predictive and multi-level proactive responses to avoid premature system failure due to occlusion-illumination coupling interference, has become a pressing problem for those skilled in the art. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to address the technical defect of existing robotic arm visual servo technology in handling dynamic occlusion, which is that it fails to couple the handling of sudden changes in illumination caused by dynamic obstacles with subsequent physical occlusion, resulting in the visual perception system failing prematurely before physical occlusion occurs. The present invention provides a robotic arm visual servo method and system that is resistant to dynamic occlusion.

[0007] To achieve the above objectives, the present invention provides the following technical solution: a visual servoing method for a robotic arm that resists dynamic occlusion, applicable to an operating system including a robotic arm body, a control unit, a main vision system, and an environmental monitoring system, wherein the main vision system includes a main servo camera and an active illumination array, and the environmental monitoring system includes an environmental perception sensor; the method includes the following steps: Step S1, Servo Task and Dynamic Environment Perception: The control unit drives the robotic arm to move toward the target object; at the same time, the environmental perception sensor scans the workspace to acquire and process the motion state of dynamic obstacles in the workspace in real time, and continuously outputs a predicted state parameter vector containing the three-dimensional model, position, velocity and acceleration of the dynamic obstacle. Step S2, Prediction of Illumination Disturbance Precursors: The control unit combines the predicted state parameter vector of the dynamic obstacle with the pose relationship between the main vision system and the target object to predict the changes in shadow or reflected light caused by the movement of the dynamic obstacle on the surface of the target object in the next time step, thereby generating a quantified, pixel-level predicted illumination disturbance map. Step S3, System Stability Assessment and Response Decision: The control unit calculates the system stability index in real time by fusing visual feature stability, illumination control margin and occlusion probability. Based on the comparison results of the system stability index with the first threshold and the second threshold, it switches between normal servo, first-level response and second-level response. Step S4, Active Illumination Compensation: When the first-level response is triggered, based on the predicted illumination perturbation map, with the goal of maintaining the uniformity and characteristic stability of the target surface illumination, the compensation driving command of the active illumination array is solved. Step S5, Path Holding and Replanning: When the Level 2 response is triggered, illumination compensation is paused, and a choice is made between a predictive path holding strategy and a local obstacle avoidance path replanning strategy based on the predicted duration of physical occlusion.

[0008] To further realize the present invention, the following technical solutions may be preferred: Preferably, step S2 specifically includes: A virtual optical scene is constructed in the control unit, with the active lighting array as the virtual light source, the three-dimensional geometric model of the dynamic obstacle determined by its predictive state parameter vector for the next time step as the light-shielding body, and the three-dimensional model of the target object as the light-receiving surface. Using a real-time shadow mapping algorithm, the distribution of shadows cast by the shading body on the light receiving plane, or the distribution of reflected light generated by the surface of the shading body on the virtual point light source, is calculated in the virtual optical scene to generate the predicted illumination perturbation map.

[0009] Preferably, step S4 specifically includes: The predicted illumination perturbation map is input into a pre-established illumination response benchmark model, which stores the mapping relationship between the driving parameters of each unit of the illumination array and the illumination distribution. An objective function is constructed to minimize the difference between the predicted illumination intensity of the compensated target surface and the reference illumination intensity during the calibration phase, with the brightness values ​​of the N light-emitting units in the active illumination array as the variables to be optimized. An optimization algorithm is used to solve the objective function to obtain a set of compensating illumination driving commands that can form a complementary light field to the predicted illumination disturbance in spatial distribution, and these commands are sent to the active illumination array.

[0010] Preferably, before performing step S1, a static illumination response model calibration step is performed, which includes: The active lighting array is controlled to illuminate the target object one by one according to a preset mode that includes independent brightness scanning of each light-emitting unit; In each lighting mode, the main servo camera acquires images of the target object and extracts the brightness distribution information of the target area in the image through the static lighting calibration module. Based on data collected under all illumination modes, the illumination response baseline model is constructed and stored, and baseline visual feature descriptors are extracted for subsequent evaluation of visual feature stability.

[0011] Preferably, the system stability index in step S3 is a weighted fusion of the following three parts: First, the visual feature stability component is quantified by comparing the matching degree, number of matches, and spatial distribution dispersion of the visual feature descriptors extracted from the current frame image with the pre-stored baseline feature descriptors. Second, the illumination control margin component is calculated by statistically analyzing the degree of closeness between the driving brightness value of each light-emitting unit and its maximum rated brightness value in the compensation illumination driving command currently issued to the active illumination array, in order to reflect the control margin of the illumination compensation system. Third, the occlusion probability component is calculated by the illumination disturbance prediction module based on the predictive state parameter vector of the dynamic obstacle, based on the probability that it will geometrically interfere with the line-of-sight cone of the main servo camera within a specific time window in the future.

[0012] Preferably, the decision logic for the hierarchical response is as follows: When the system stability index is higher than the first threshold, the system is in a stable servo state and performs regular vision servoing. When the light level is below the first threshold but above the second threshold, the system determines that the light disturbance is the main threat, activates and executes step S4 as the first-level response; When the light level falls below the second threshold, the system determines that the lighting compensation has reached its limit or that physical occlusion is unavoidable, suspends the lighting compensation, and executes step S5 as the second-level response.

[0013] Preferably, step S5 specifically includes: Predict the duration of occlusion of the target object based on the state parameter vector of the dynamic obstacle; If the predicted occlusion duration is less than the preset waiting time threshold, a predictive path-keeping strategy is executed. The robotic arm body suspends visual servo guidance and extrapolates the trajectory based on its velocity and acceleration state just before the occlusion occurs in order to maintain a brief inertial motion. If the predicted occlusion duration is greater than or equal to the waiting time threshold, a local obstacle avoidance path replanning strategy is executed. The dynamic obstacle and its predicted trajectory are treated as dynamic obstacle bodies. Using the path planner, a collision-free local obstacle avoidance path is recalculated in the joint space of the robotic arm, and the robotic arm body is driven to move along the new path.

[0014] Preferably, the process of obtaining the predictive state parameter vector of the dynamic obstacle in step S1 specifically includes: The environmental perception sensor is used to collect three-dimensional point cloud data covering the entire workspace in real time. Point cloud clusters representing the dynamic obstacles are identified and separated from the three-dimensional point cloud data using point cloud clustering and segmentation algorithms. The Kalman filter algorithm is used to process the point cloud clusters of identified dynamic obstacles to continuously track and predict their motion state, thereby outputting a state parameter vector containing a three-dimensional geometric model, centroid position, velocity vector, and acceleration vector.

[0015] A vision servoing system for a robotic arm that resists dynamic occlusion includes a robotic arm body, an end effector, and a control unit, and further includes: The main vision system includes a main servo camera and an active illumination array. The main servo camera is used to acquire images of the target object in real time, and the active illumination array is used for active illumination and consists of multiple light-emitting units whose brightness can be controlled independently. Environmental monitoring system, including environmental sensing sensors for acquiring three-dimensional geometric information and dynamic obstacle information of the workspace; The control unit includes the following modules: The dynamic environment perception module is used to receive and process data from the environment perception sensor in order to identify, track and predict the state parameter vector of the dynamic obstacle; The lighting disturbance prediction module is used to predict the lighting disturbance map that the dynamic obstacle will cause on the surface of the target object by physical rendering calculation based on the state parameter vector. The illumination compensation calculation module is used to solve and output the compensation illumination driving command for the active illumination array based on the predicted illumination perturbation map when the first-level response command is received.

[0016] Preferably, the main servo camera is a high-speed industrial camera with a global shutter function, the active illumination array is a ring structure and is coaxially arranged around the lens of the main servo camera, and the environmental perception sensor is a time-of-flight 3D camera.

[0017] The beneficial effects of this invention are: This invention, through its unique forward-looking prediction and active compensation mechanism for illumination disturbances, as well as its hierarchical response strategy based on comprehensive system state assessment, can greatly improve the robustness and success rate of visual servoing tasks, especially when facing the dual challenges of illumination and physics caused by dynamic occlusion. It effectively solves the technical pain point of visual perception failure caused by sudden changes in illumination in the prior art, and ensures the continuity and reliability of servoing tasks in complex dynamic environments. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the overall structure of the system of the present invention.

[0019] Figure 2 This is a functional block diagram of the system of the present invention.

[0020] Figure 3 This is a flowchart illustrating the method of the present invention.

[0021] Figure 4 This is a schematic diagram illustrating the principle of illumination disturbance prediction in this invention.

[0022] Figure 5 This is a schematic diagram illustrating the active illumination compensation principle of the present invention.

[0023] Figure 6 This is a schematic diagram of the hierarchical response decision-making mechanism of the present invention.

[0024] Figure 7 This is a schematic diagram of the path preservation and replanning strategy of the present invention. Detailed Implementation

[0025] In the description of this invention, it should also be noted that, unless otherwise explicitly specified and limited, the terms "set," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

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

[0027] like Figure 1 The diagram shown illustrates the overall functional structure of a multi-channel fluorescence detection system based on dynamic crosstalk correction provided by this invention. This invention also provides a dynamic occlusion-resistant visual servoing system for a robotic arm, comprising a robotic arm body, an end effector, and a control unit. The system further includes: The main vision system is fixedly mounted on the end effector of the robotic arm or on a fixed bracket independent of the robotic arm. The main vision system includes: The main servo camera is a high-speed industrial camera with a global shutter function. Its optical lens faces the working area of ​​the robotic arm and is used to acquire high frame rate and high resolution images of the target object in real time. The image data stream is then transmitted to the control unit through the first data interface. The active illumination array has a ring-shaped physical structure and is coaxially arranged around the lens of the main servo camera. The active illumination array consists of at least 64 white light-emitting diode (LED) units that can be independently controlled by pulse width modulation (PWM), with each LED unit having a brightness adjustment resolution of at least 10 bits. The illumination array is connected to the control output port of the control unit via a multi-channel driver, receiving and executing unit-by-unit brightness adjustment commands from the control unit.

[0028] The environmental monitoring system has a field of view that fully covers the workspace of the robotic arm. The environmental monitoring system includes: The environmental perception sensor is a time-of-flight (ToF) 3D camera with a depth image resolution of at least VGA (640×480), a depth measurement accuracy of better than 1% at a distance of 1 meter, and a data output frame rate of at least 30 Hz. The environmental perception sensor transmits its acquired 3D point cloud data to the control unit in real time via a second data interface.

[0029] like Figure 2 As shown, the control unit is an industrial computer or embedded controller integrating a central processing unit (CPU) and a graphics processing unit (GPU). It internally stores and runs control programs, which are instantiated within the control unit as a series of cooperating functional modules, including: The static illumination calibration module is used to control the active illumination array and the main servo camera to work together before the mission begins, collect data, and build a benchmark model that maps the driving parameters of each light-emitting unit to the illumination response of the target object surface.

[0030] The dynamic environment perception module is used to receive and process 3D point cloud data from the environment perception sensor. Through point cloud segmentation, clustering and state estimation filtering algorithms, it identifies, tracks and predicts the state parameter vector of dynamic obstacles in real time. The state parameter vector includes the obstacle's 3D geometric contour, centroid position, linear velocity, angular velocity and acceleration.

[0031] The illumination perturbation prediction module's computational tasks are scheduled to be executed on the graphics processing unit (GPU). This module receives the state parameter vector of a dynamic obstacle, and based on a real-time shadow mapping physical rendering algorithm, it forward-predicts the pixel-level illumination perturbation distribution that the obstacle will cause on the surface of the target object in the next time step, and generates a quantized predicted illumination perturbation map (PPDM).

[0032] The illumination compensation solution module is also scheduled to execute on the graphics processing unit (GPU). This module takes the predicted illumination perturbation map as input and aims to minimize the compensated illumination deviation. Based on the illumination response baseline model, it solves and outputs a set of precise brightness drive instructions for each light-emitting unit of the active illumination array that can offset the predicted perturbation.

[0033] The visual servoing and feature stability evaluation module processes the image data stream from the main servo camera. On one hand, it executes image-based visual servoing (IBVS) or position-based visual servoing (PBVS) algorithms to generate basic motion commands for the robotic arm. On the other hand, it extracts the visual features of the current image, compares them with the baseline features, and quantifies and outputs the feature stability components that reflect the current visual perception quality.

[0034] The integrated decision-making and path planning module serves as the system's central hub. This module integrates the calculation logic for the System Stability Index (SSI), evaluating the overall system state in real time by weighted fusion of characteristic stability components, illumination control margin components fed back from the illumination compensation solution module, and physical occlusion probability components calculated by the illumination disturbance prediction module. Based on the comparison between the SSI value and a preset threshold, this module deterministically and seamlessly switches between three operating modes: stable servo, active illumination compensation, and path maintenance and replanning. Internally, this module also integrates a path planner based on the RRT* algorithm, which, upon receiving a path replanning command, can quickly generate a collision-free obstacle avoidance path based on predicted information about dynamic obstacles.

[0035] The control unit achieves deterministic low-latency data exchange and control command issuance with the controller of the robotic arm body, the main servo camera and active lighting array driver of the main vision system, and the environmental perception sensor of the environmental monitoring system through a high-speed industrial bus. This forms a closed-loop vision servo control loop with forward-looking compensation for lighting disturbances and multi-level failure protection.

[0036] like Figure 1 and Figure 2 The diagram shows the overall physical architecture and system module architecture of the dynamic occlusion-resistant robotic arm visual servo system in this embodiment of the invention. This system is applied in a typical industrial automation work unit, aiming to drive a robotic arm so that its end effector can smoothly and stably interact with target objects arranged within its workspace, such as grasping, assembling, and inspecting, even in the presence of unpredictable moving obstacles. The entire system uses a central control unit as its core, controlling and integrating all subsystems within the system.

[0037] Specifically, the robotic arm itself can be any six-axis multi-joint industrial robot, such as the Universal Robots UR10e model, with a repeatability of at least 0.05 mm. The end effector varies depending on the specific servo task and can be a pneumatic or electric gripper, a suction cup, or a specialized tool for assembly. The object is the target of the servo task, and its surface must be suitable for stable recognition by the vision system. Dynamic obstacles are any unstable objects that may enter the robotic arm's workspace and interfere with the vision servoing process; typical examples include a service worker's hand, a passing material cart, or other moving equipment.

[0038] The perception system is fundamental to the robot's environmental perception and consists of two vision systems: a primary vision system and an environmental vision system. The primary vision system is tightly integrated and can be either fixedly mounted on the end effector of the robot body or independently mounted above the workspace, depending on application requirements, to obtain a spatially stable field of view throughout the work area. The primary vision system includes a primary servo camera and an active illumination array. In one embodiment, the primary servo camera uses a Basler ac A2440-75µm industrial camera with a Sony IMX250 CMOS sensor, featuring a resolution of 2448×2048 and a global shutter speed to avoid rolling shutter effects when capturing fast-moving objects. The primary servo camera connects to the control unit via a CoaXPress-12 interface and transmits uncompressed raw image data at rates up to 75 frames per second. The active illumination array is physically structured on a circular printed circuit board (PCB) with an outer diameter of 150 mm and an inner diameter of 70 mm. The PCB substrate is a copper-clad aluminum substrate with high thermal conductivity to ensure efficient heat dissipation. On a ring-shaped PCB, a total of N=128 independently addressed high-brightness white light-emitting diode (LED) units are distributed in concentric circles on two layers. Each LED unit uses Cree's XQ-E series and has an individual miniature Fresnel lens, providing precise control over the beam divergence angle to focus light energy onto the target working area. These 128 LED units are driven by two cascaded Texas Instruments TLC5955 64-channel constant current LED drivers, with high-speed data writing by the control unit via a serial peripheral interface. Each channel provides a 16-bit pulse width modulation depth, enabling precise control over the brightness of each LED unit.

[0039] The environmental monitoring system should have a viewpoint that comprehensively covers all accessible workspace of the robotic arm. This system includes environmental perception sensors tasked with acquiring three-dimensional geometric information of the workspace, particularly the presence, location, and movement trends of moving obstacles. In this example, the environmental perception sensor 7 is a Microsoft Azure Kinect DK device that integrates a time-of-flight (ToF) depth sensor. This ToF sensor provides depth imaging in a wide field-of-view mode, with a depth measurement accuracy better than 10 mm (1 meter distance) and a data output frame rate of 30 Hz. It transmits 3D point cloud data signals to the control unit in real time via its built-in USB interface.

[0040] The control unit is the brain of the entire system. The control unit is housed in an industrial computer. This computer's core includes an Intel Core i7-8665ue quad-core, eight-thread processor, 32GB of DDR4 industrial-grade memory, and an integrated Tesla T4 graphics processing unit (GPU). The control unit communicates with the robotic arm controller, the active lighting array 5's drivers, and other I / O modules at high speed and with a communication cycle as low as 1 millisecond to ensure strict real-time and synchronization of the entire control loop. Example 2

[0041] Based on the system architecture constructed above, referring to Figures 3 to 7 This paper provides a detailed, flowchart-based explanation of the specific steps of a visual servoing method for robotic arms that resists dynamic occlusion, as proposed in this invention.

[0042] First, the static illumination response model calibration step is performed. Before the servo task starts, the control unit drives the active illumination array to illuminate the target object located in the robotic arm's workspace one by one using a preset series of illumination modes. The active illumination array is an array composed of N independently addressable and controllable light-emitting units. In each illumination mode, the main servo camera acquires an image of the target object, and the static illumination calibration module in the control unit extracts the brightness distribution information of the target area in the image and a set of reference visual feature descriptors. Based on the data acquired in all illumination modes, the control unit constructs an illumination response reference model that maps the driving parameters of each light-emitting unit to the light intensity distribution and visual feature stability of the target object's surface, and stores this model in the control unit's non-volatile memory.

[0043] Secondly, the servo task and dynamic environment continuous perception steps are executed. Based on images acquired in real-time by the main servo camera, the control unit calculates the robotic arm's motion commands using a visual servoing algorithm, driving the robotic arm towards the target object. Simultaneously, the environmental perception sensor in the environmental monitoring system continuously performs a 3D scan of the robotic arm's workspace, acquiring real-time 3D point cloud data of dynamic objects within the workspace. The environmental perception sensor is a time-of-flight camera with a depth resolution of at least 640x480 pixels and a frame rate of at least 30 Hz. The dynamic environment perception module within the control unit receives and processes the 3D point cloud data. Through point cloud clustering and segmentation algorithms, it identifies dynamic obstacles entering the preset warning area and uses a Kalman filter algorithm to track and predict the motion state of the identified dynamic obstacles, continuously outputting a state parameter vector containing the obstacle's 3D geometric model, center of mass position, velocity vector, and acceleration vector.

[0044] Again, refer to Figure 4The system performs a quantitative prediction and evaluation step for the precursors of lighting disturbances. The lighting disturbance prediction module within the control unit, based on the state parameter vector of the dynamic obstacle, especially its predicted position and orientation at the next time step (t+Δt), and combined with the relative spatial pose relationship between the main vision system and the target object in the calibrated workspace coordinate system, performs a forward physical rendering calculation. Specifically, this calculation constructs a virtual optical scene with each emitting unit of the active lighting array as a virtual point light source, the 3D geometric model of the dynamic obstacle as a light-blocking body, and the surface of the target object as the receiving plane. Using a real-time shadow mapping algorithm, it calculates the distribution of the shadows cast on the target object's surface by the movement of the dynamic obstacle or the brightness changes caused by surface reflection at the next time step (t+Δt), thereby generating a quantized, pixel-level predicted lighting disturbance map (PPDM). The PPDM accurately describes the upcoming brightness gain or attenuation values ​​in various regions of the target object's surface.

[0045] Furthermore, referring to Figure 5 When the energy norm of the predicted illumination perturbation map exceeds a preset perturbation trigger threshold, an active illumination compensation step is executed. The illumination compensation solution module within the control unit takes the predicted illumination perturbation map as input and aims to maintain the uniformity of illumination on the target object surface and the stability of the baseline visual features. Based on a pre-established illumination response baseline model, it solves an inverse compensation problem. Specifically, this solution process involves constructing an objective function that minimizes the difference between the compensated predicted illumination intensity of the target surface and the baseline illumination intensity during the calibration phase, while using the brightness values ​​of N emitting units in the active illumination array as variables to be optimized. Through numerical optimization algorithms such as gradient descent or conjugate gradient, the specific brightness value that each emitting unit needs to adjust to compensate for the predicted illumination perturbation is calculated, thereby generating a set of compensation illumination driving commands. The control unit sends the set of instructions to the drive circuit of the active illumination array via the control bus, and adjusts the output luminous flux of each light-emitting unit in real time and with high dynamics to form a light field that is complementary to the spatial distribution of the predicted disturbance. This allows the active cancellation of the dynamic illumination disturbance when it actually occurs, ensuring the quality and stability of the images acquired by the main servo camera.

[0046] Furthermore, the system performs continuous assessments of its overall stability and implements tiered response decision-making steps. Throughout the execution of the servo task, the integrated decision-making module within the control unit calculates a system stability index in real time. The system stability index is a normalized scalar value, which is a weighted fusion of the following three components: First, the visual feature stability component, provided by the visual servoing and feature stability evaluation module, is calculated as the ratio of the number of stable feature points matched in the current frame to the total number of baseline features, multiplied by a confidence factor related to the average matching distance of the feature descriptors; Second, the illumination control margin component, fed back by the illumination compensation solution module, has a value of 1-(max(d'_i) / d_max)^p, where d'_i is the current driving value of each LED, d_max is its maximum rated value, and p is an exponent greater than 1 (e.g., 2). This component reflects whether the illumination compensation system has approached the upper limit of its adjustment capability; Third, the physical occlusion probability component, calculated by the illumination disturbance prediction module, is obtained by geometrically intersecting the predicted trajectory of the obstacle over a future period of time (e.g., 1 second) with the line-of-sight cone of the main servo camera.

[0047] The comprehensive decision-making module, based on the value of the system stability index, refers to... Figure 6 The system executes the following tiered response strategy: When the system stability index value is higher than the first preset threshold, the system is in a stable servo state and only performs regular visual servo control; when the system stability index value drops and falls below the first preset threshold, but is still higher than the second preset threshold, the system determines that the illumination disturbance is the main threat, activates and executes the aforementioned active illumination compensation step, which is the first level response; when the system stability index value further drops and falls below the second preset threshold, the system determines that the illumination compensation has reached its limit or that physical occlusion is unavoidable, then immediately suspends active illumination compensation and switches to the second level response, namely the path preservation and replanning step.

[0048] Finally, upon triggering the second-level response, the path preservation and replanning steps are executed. (See reference...) Figure 7The integrated decision-making module first accurately predicts the duration and spatial range of the occlusion of the target object, based on the state parameter vector and 3D geometric model of the dynamic obstacle. If the predicted occlusion duration is less than a preset waiting time threshold, the system executes a predictive path-keeping strategy: the control unit instructs the end effector of the robotic arm to pause its visually servo-guided movement and extrapolates its trajectory based on its velocity and acceleration state just before the occlusion occurs, maintaining a brief, low-speed uniform or uniformly accelerated motion to quickly resume tracking after the occlusion ends. If the predicted occlusion duration is greater than or equal to the waiting time threshold, or if the obstacle's trajectory will block the path to the target for an extended period, the system immediately executes a local obstacle avoidance path replanning strategy: the control unit inputs the 3D geometric model of the dynamic obstacle and its predicted trajectory as a dynamic obstacle into the built-in path planner. The path planner uses a dynamic variant based on a fast extended random tree star topology algorithm to recalculate a collision-free, time-optimal, or energy-optimal local path from the current position to the target object within the joint space of the robotic arm, with the constraint of avoiding the dynamic obstacle. Once the calculation is complete, the control unit immediately drives the robotic arm to move along the newly planned path to bypass dynamic obstacles. After the bypass is completed, the visual servo is reactivated to continue the subsequent tasks. Example 3

[0049] To verify the practical effectiveness of the method and system of this invention, a specific experiment was conducted. The experimental environment was as follows: a UR10e robotic arm, an Azure Kinect DK as the environmental perception sensor, a Basler acA2440-75um camera and a custom 128-unit LED ring light source as the main vision system, and an industrial computer equipped with a Tesla T4 GPU as the control unit. The task was to have the robotic arm precisely grasp a synchronizer gear ring (target object) from a parts box, the surface of which has reflective surfaces and complex grooves.

[0050] In this experiment, the operator's arm (a dynamic obstacle) cuts into the workspace from the right side at a speed of approximately 0.5 m / s, traversing between the main servo camera and the target object. The shadow cast by the arm's movement is already cast at an almost constant speed in the initial stage of the movement, before it enters the toothed ring surface. The arm's movement then casts a fast-moving shadow on the toothed ring momentarily, after which the arm will completely obscure the toothed ring (for approximately 1.5 seconds).

[0051] The response procedure using the invented system is as follows: 1. When the arm enters a 1.5-meter range within the workspace, the dynamic environment perception module identifies the arm and begins to track its movement using a Kalman filter.

[0052] 2. When the arm is approximately 0.8m away from the target object, the illumination perturbation prediction module calculates based on the arm's predicted motion trajectory that the arm will cast a large shadow on the right half of the target toothed ring in about 150 milliseconds. The resulting PPDM image has an energy norm exceeding the set threshold.

[0053] 3. At this point, the system's stability index drops from the initial value of 0.98 to 0.82, which is below the first threshold (0.85), and the system enters the first-level response. The illumination compensation solution module is activated.

[0054] 4. The module calculates a set of illumination compensation instructions in 3 milliseconds: instructs the brightness of about 40 LED units on the left side of the illumination array to be increased to between 30% and 70%, while the brightness of the LED units located on the arm that block the light path is reduced.

[0055] 5. When the arm's shadow is actually cast onto the toothed ring, the compensation light field precisely compensates for the brightness reduction caused by the shadow, maintaining a high degree of uniformity and stability in the brightness of the image captured by the main servo camera. The visual feature stability component consistently remains above 0.9. Visual servoing is uninterrupted.

[0056] 6. As the arm continues to move, the illumination disturbance prediction module calculates that the occlusion probability component will rise sharply from the initial value of 0.1 to 0.95 within 100 milliseconds.

[0057] 7. The system stability index value calculated by the integrated decision module drops to 0.4 instantly, which is lower than the second threshold (0.5), triggering the second-level response.

[0058] 8. The module predicts that the physical occlusion will last for approximately 1.5 seconds, which is greater than the set waiting time threshold (1.2 seconds). The system decides to perform path replanning.

[0059] 9. The path planner is invoked, using the predicted spatiotemporal trajectory of the arm as a dynamic obstacle. Within approximately 25 milliseconds, the planner generates a new path that guides the robotic arm's end effector to lift 5 centimeters upward and then translate 10 centimeters, bypassing the arm's movement area in an overpass-like manner.

[0060] 10. The control unit switched to trajectory tracking mode, driving the robotic arm to smoothly perform obstacle avoidance maneuvers.

[0061] 11. After the arm disengaged, the environmental monitoring system confirmed the threat was eliminated. The system automatically switched back to visual servo mode, the main servo camera re-locked onto the target toothed ring, and completed the final alignment and grasp within an additional 0.5 seconds. The entire mission was successfully completed. Example 4

[0062] When executing step S3, the control unit is further configured to: First, a series of consecutive historical values ​​of the system stability index are recorded within a sliding window of fixed time length; Secondly, by performing linear trend analysis on the historical value series, the instantaneous rate of change that quantifies the current rate of decline or rise of the index is calculated; Then, based on the instantaneous rate of change, the decision threshold used to trigger the second-level response is dynamically adjusted. The specific adjustment rule is: when the index is trending downward, its actual trigger threshold will be higher than the preset static threshold, and the faster the index drops, the greater the increase in the trigger threshold. Ultimately, by comparing the real-time system stability index with this dynamically raised decision threshold, a predictive judgment of the system state can be made, thereby triggering a second-level response in advance when the index deteriorates sharply.

[0063] By introducing the concept of rate of change, the system no longer reacts to the currently observed state, but rather to the rate of future deterioration. Even if the stability index has not yet fallen below the threshold, if it is experiencing a precipitous drop, the system can still make a more advanced response in advance. This shift from static to dynamic decision-making significantly enhances the system's intelligence and foresight.

[0064] Between steps S4 and S5, there is also an intermediate response step, which is activated when the system stability index falls within the warning range. The intermediate response step specifically includes the following: (a) Establish a nonlinear optimization problem with the two-dimensional or three-dimensional velocity increments (Δv_x, Δv_y, Δv_z) of the end effector of the robotic arm in the task space as optimization variables; (b) The objective function of the nonlinear optimization problem is to maximize the predicted collision time, which is obtained by extrapolating the intersection of the predicted trajectory of the dynamic obstacle and the predicted trajectory of the main servo camera after velocity increment adjustment. (c) The nonlinear optimization problem has the following constraints: i. The L2 norm of the speed increment (Δv) must be less than the preset maximum perturbation speed limit, which is set to no more than 15% of the normal servo task speed; ii. The visual feature error increment caused by the end-effector pose after speed increment adjustment must not exceed the maximum tolerance error; (d) Use the Sequential Quadratic Programming (SQP) algorithm to quickly solve the nonlinear optimization problem, obtain the optimal speed increment (Δv), and superimpose the optimal speed increment onto the original visual servo control command to drive the robotic arm to perform a small evasive motion.

[0065] An intermediate response is inserted between the original two-level response. This makes the overall system's response hierarchy smoother and more efficient, avoiding abrupt switching between dimming and large-scale detours at critical states.

[0066] The above are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A dynamic occlusion-resistant visual servoing method for a robotic arm, applicable to an operating system comprising a robotic arm body, a control unit, a main vision system, and an environmental monitoring system, wherein the main vision system includes a main servo camera and an active illumination array, and the environmental monitoring system includes environmental sensing sensors, characterized in that, The method includes the following steps: Step S1, Servo Task and Dynamic Environment Perception: The control unit drives the robotic arm to move toward the target object; at the same time, the environmental perception sensor scans the workspace to acquire and process the motion state of dynamic obstacles in the workspace in real time, and continuously outputs a predicted state parameter vector containing the three-dimensional model, position, velocity and acceleration of the dynamic obstacle. Step S2, Prediction of Illumination Disturbance Precursors: The control unit combines the predicted state parameter vector of the dynamic obstacle with the pose relationship between the main vision system and the target object to predict the changes in shadow or reflected light caused by the movement of the dynamic obstacle on the surface of the target object in the next time step, thereby generating a quantified, pixel-level predicted illumination disturbance map. Step S3, System Stability Assessment and Response Decision: The control unit calculates the system stability index in real time by fusing visual feature stability, illumination control margin and occlusion probability. Based on the comparison results of the system stability index with the first threshold and the second threshold, it switches between normal servo, first-level response and second-level response. Step S4, Active Illumination Compensation: When the first-level response is triggered, based on the predicted illumination perturbation map, with the goal of maintaining the uniformity and characteristic stability of the target surface illumination, the compensation driving command of the active illumination array is solved. Step S5, Path Holding and Replanning: When the Level 2 response is triggered, illumination compensation is paused, and a choice is made between a predictive path holding strategy and a local obstacle avoidance path replanning strategy based on the predicted duration of physical occlusion.

2. The method according to claim 1, characterized in that, Step S2 specifically includes: A virtual optical scene is constructed in the control unit, with the active lighting array as the virtual light source, the three-dimensional geometric model of the dynamic obstacle determined by its predictive state parameter vector for the next time step as the light-shielding body, and the three-dimensional model of the target object as the light-receiving surface. Using a real-time shadow mapping algorithm, the distribution of shadows cast by the shading body on the light receiving plane, or the distribution of reflected light generated by the surface of the shading body to the virtual light source, is calculated in the virtual optical scene to generate the predicted illumination perturbation map.

3. The method according to claim 1, characterized in that, Step S4 specifically includes: The predicted illumination perturbation map is input into a pre-established illumination response benchmark model, which stores the mapping relationship between the driving parameters of each unit of the illumination array and the illumination distribution. An objective function is constructed to minimize the difference between the predicted illumination intensity of the compensated target surface and the reference illumination intensity during the calibration phase, with the brightness values ​​of the N light-emitting units in the active illumination array as the variables to be optimized. An optimization algorithm is used to solve the objective function to obtain a set of compensating illumination driving commands that can form a complementary light field to the predicted illumination disturbance in spatial distribution, and these commands are sent to the active illumination array.

4. The method according to claim 3, characterized in that, Before performing step S1, a static illumination response model calibration step is performed, which includes: The active lighting array is controlled to illuminate the target object one by one according to a preset mode that includes independent brightness scanning of each light-emitting unit; In each lighting mode, the main servo camera acquires images of the target object and extracts the brightness distribution information of the target area in the image through the static lighting calibration module. Based on data collected under all illumination modes, the illumination response baseline model is constructed and stored, and baseline visual feature descriptors are extracted for subsequent evaluation of visual feature stability.

5. The method according to claim 1, characterized in that, In step S3, the system stability index is a weighted fusion of the following three parts: First, the visual feature stability component is quantified by comparing the matching degree, number of matches, and spatial distribution dispersion of the visual feature descriptors extracted from the current frame image with the pre-stored baseline feature descriptors. Second, the illumination control margin component is calculated by statistically analyzing the degree of closeness between the driving brightness value of each light-emitting unit and its maximum rated brightness value in the compensation illumination driving command currently issued to the active illumination array, in order to reflect the control margin of the illumination compensation system. Third, the occlusion probability component is calculated based on the predictive state parameter vector of the dynamic obstacle, determining the probability that it will geometrically interfere with the line-of-sight cone of the main servo camera within a specific future time window.

6. The method according to claim 5, characterized in that, The decision-making logic for a tiered response is as follows: When the system stability index is higher than the first threshold, the system is in a stable servo state and performs regular vision servoing. When the light level is below the first threshold but above the second threshold, the system determines that the light disturbance is the main threat, activates and executes step S4 as the first-level response; When the light level falls below the second threshold, the system determines that the lighting compensation has reached its limit or that physical occlusion is unavoidable, suspends the lighting compensation, and executes step S5 as the second-level response.

7. The method according to claim 1, characterized in that, Step S5 specifically includes: Predict the duration of occlusion of the target object based on the state parameter vector of the dynamic obstacle; If the predicted occlusion duration is less than the preset waiting time threshold, a predictive path-keeping strategy is executed. The robotic arm body suspends visual servo guidance and extrapolates the trajectory based on its velocity and acceleration state just before the occlusion occurs in order to maintain a brief inertial motion. If the predicted occlusion duration is greater than or equal to the waiting time threshold, a local obstacle avoidance path replanning strategy is executed. The dynamic obstacle and its predicted trajectory are treated as dynamic obstacle bodies. Using the path planner, a collision-free local obstacle avoidance path is recalculated in the joint space of the robotic arm, and the robotic arm body is driven to move along the new path.

8. The method according to claim 1, characterized in that, The process of obtaining the predictive state parameter vector of the dynamic obstacle in step S1 specifically includes: The environmental perception sensor is used to collect three-dimensional point cloud data covering the entire workspace in real time. Point cloud clusters representing the dynamic obstacles are identified and separated from the three-dimensional point cloud data using point cloud clustering and segmentation algorithms. The Kalman filter algorithm is used to process the point cloud clusters of identified dynamic obstacles to continuously track and predict their motion state, thereby outputting a state parameter vector containing a three-dimensional geometric model, centroid position, velocity vector, and acceleration vector.

9. A dynamic occlusion-resistant visual servo system for a robotic arm, used to implement the method as described in any one of claims 1-8, the system comprising a robotic arm body, an end effector, and a control unit, characterized in that, Also includes: The main vision system includes a main servo camera and an active illumination array. The main servo camera is used to acquire images of the target object in real time, and the active illumination array is used for active illumination and consists of multiple light-emitting units whose brightness can be controlled independently. Environmental monitoring system, including environmental sensing sensors for acquiring three-dimensional geometric information and dynamic obstacle information of the workspace; The control unit includes the following modules: The dynamic environment perception module is used to receive and process data from the environment perception sensor in order to identify, track and predict the state parameter vector of the dynamic obstacle; The lighting disturbance prediction module is used to predict the lighting disturbance map that the dynamic obstacle will cause on the surface of the target object by physical rendering calculation based on the state parameter vector. The illumination compensation calculation module is used to solve and output the compensation illumination driving command for the active illumination array based on the predicted illumination perturbation map when the first-level response command is received.

10. The system according to claim 9, characterized in that, The main servo camera is a high-speed industrial camera with a global shutter function. The active illumination array is a ring structure and is coaxially arranged around the lens of the main servo camera. The environmental perception sensor is a time-of-flight 3D camera.

Images