A control barrier function based dynamic occlusion avoidance method and system for robot visual servoing

Abstract

The application discloses a kind of mechanical arm visual servo dynamic shelter avoidance method and system based on control barrier function, by establishing the kinematic model of mechanical arm and the visual system model including end camera and fixed camera;Design nominal visual servo controller to obtain nominal joint angular velocity;Dynamic barrier is projected to image plane, and control barrier function for quantifying the safety distance between reference feature point and barrier projection is constructed, and the inequality constraint of nominal joint angular velocity is obtained by derivation;The motion state of barrier is estimated on-line by extended Kalman filter state estimator using fixed camera and input to inequality constraint;With the minimum actual control amount and nominal joint angular velocity difference as the goal, construct quadratic programming problem with inequality constraint as safety condition and solve, output optimal joint angular velocity.The application can directly solve the feature point shelter problem in image space under unknown dynamic barrier scene, guarantee the continuity and safety of visual servo task.

Description

Technical Field

[0001] This invention relates to the field of visual servo control, and more specifically to a method and system for dynamic occlusion avoidance of a robotic arm based on a control obstacle function. Background Technology

[0002] Visual servoing technology controls robot movement in real time through visual information feedback and is widely used in industrial automation, service robots and human-robot collaboration. Among them, image-based visual servoing methods have become the mainstream solution due to their strong robustness.

[0003] Existing visual servoing methods typically assume that reference feature points are always visible. However, in dynamic environments, the robot's own movement or the movement of external objects can easily cause the reference feature points to be occluded, resulting in visual measurement failure, decreased control performance, or even system instability.

[0004] To address the occlusion problem, existing technologies mainly fall into three categories: First, geometric obstacle avoidance methods construct obstacle models in joint space or Cartesian space to prevent physical collisions between the robot and obstacles, but do not consider image-level occlusion issues; second, model predictive control methods assess occlusion risks by predicting the motion of the robot and obstacles and adjust the trajectory in advance, but require repeated optimization problems, are computationally complex, and rely on accurate models; and third, obstacle function control methods construct safety constraints in joint space or Cartesian space for obstacle avoidance, but rarely model feature point occlusion at the image level.

[0005] However, the above solutions have the following problems: geometric obstacle avoidance cannot guarantee the visibility of visual feature points; model prediction control has a large computational load and is highly dependent on the obstacle motion model, making it difficult to handle unknown dynamic obstacles in real time; existing obstacle control function methods are mostly oriented towards physical collisions and do not directly constrain the occlusion problem from the image space, and some methods assume that the obstacle motion state is known, making it difficult to adapt to practical applications. Summary of the Invention

[0006] To address the problems mentioned in the prior art, this invention proposes a dynamic occlusion avoidance method and system for visual servoing of robotic arms based on control obstacle functions. This method can directly constrain feature point occlusion in the image space when dynamic obstacles exist and their motion state is unknown, thereby achieving a balance between visual servoing tasks and safety.

[0007] To achieve the above objectives, the present invention adopts the following technical solution: This invention proposes a dynamic occlusion avoidance method for robotic arms based on a control obstacle function. It is applied to scenarios where dynamic objects occlude reference feature points within the field of view of the end-effector camera in the visual servo control of a robotic arm. The method includes the following steps: S1. Establish a kinematic model of the robotic arm and a vision system model. The vision system model includes an end-effector camera and a fixed camera. Based on the end-effector camera extrinsic parameters and the kinematic model of the robotic arm, construct the transformation relationship between the robotic arm and joint motion and the end-effector camera motion speed. S2. Based on the transformation relationship and the image information of the end camera, set the expected position of the reference feature point for the visual task, design the nominal visual servo controller, and calculate the nominal joint angular velocity of the robotic arm based on the image error between the current position of the reference feature point and the expected position of the reference feature point. S3. Construct safety constraints based on the control obstacle function, project the dynamic obstacle onto the image plane, construct a control obstacle function in the image space to quantify the safe distance between the reference feature point and the obstacle projection, and differentiate the control obstacle function to obtain the inequality constraints on the nominal joint angular velocity of the robotic arm. S4. Observe the dynamic obstacles using a fixed camera, estimate the motion state of the obstacles online using an extended Kalman filter state estimator, and input the estimated obstacle velocity information into the inequality constraint. S5. Within each control cycle, the optimization objective is to minimize the difference between the actual control quantity and the nominal joint angular velocity. An inequality constraint is used as a safety condition to construct and solve a quadratic programming problem, obtain the optimal joint angular velocity, and output it to the robotic arm for execution.

[0008] As a further improvement of the present invention, the process of S1 includes: A kinematic model of a robotic arm with multiple rotary joints is established, and the pose of the robotic arm end effector in the base coordinate system is described by the joint angle vector. The end-effector camera is fixedly mounted on the end of the robotic arm, and the transformation matrix from the end-effector camera coordinate system to the end-effector coordinate system is obtained through hand-eye calibration. The fixed camera is fixedly installed above the workspace of the robotic arm, and the transformation matrix from the fixed camera coordinate system to the base coordinate system of the robotic arm is calibrated. Extract at least one reference feature point from the image plane of the end camera, and define the expected pixel coordinates and real-time observed pixel coordinates of the reference feature point on the image plane.

[0009] As a further improvement of the present invention, the process of S2 includes: Based on the intrinsic parameters of the end-effector camera and the image Jacobian matrix, a mapping relationship is established between the real-time observed pixel coordinates of the reference feature points and the angular velocities of the robotic arm joints. Based on the pixel error between the real-time observed pixel coordinates and the desired pixel coordinates, a control law is designed to make the pixel error exponential converge, and the nominal joint angular velocity vector is calculated.

[0010] As a further improvement of the present invention, the process of S3 includes: Dynamic obstacles in the environment are abstracted as three-dimensional spheres, which have a center position and a radius. The three-dimensional sphere is projected onto the image plane using an end-point camera to obtain the coordinates of the obstacle projection center and the obstacle projection radius; On the image plane, a control obstacle function is constructed for each reference feature point and each obstacle projection region; By taking the time derivative of the control obstacle function, an inequality constraint is obtained regarding the nominal joint angular velocity of the robotic arm.

[0011] As a further improvement of the present invention, the expression of the control barrier function is as follows:

[0012] In the formula: For a moment Real-time observation of pixel coordinates of reference feature points; For a moment The coordinates of the obstacle's projection center; The radius of the obstacle's projection; To allow for a safe distance; This indicates that the feature point is outside the spherical obstacle and is not occluded; when When the feature point is located on the safety boundary; when When a feature point enters the safety boundary, it will be occluded by an obstacle; once occluded, the feature point's position in the camera image will be invisible. Unknown, then Unable to define; The expression for the inequality constraint is as follows:

[0013] In the formula: From Mapped to A strictly monotonically increasing continuous function, and , , It can be considered a constant; The time derivative of the control barrier function; Let be the nominal joint angular velocity vector of the robotic arm, where For the first robotic arm The angle of each joint; The expression for the time derivative of the control barrier function is as follows:

[0014]

[0015] In the formula: The Jacobian matrix of the feature point image; Let be the Jacobian matrix of the obstacle center point image; To connect the rate of change of the obstacle's projected radius with the control quantity Jacobian matrix; The velocity at which the center point is projected in the image due to the obstacle's own motion; The rate of change of the radius of the projected circle in the image caused by the movement of the obstacle itself.

[0016] As a further improvement of the present invention, the process of S4 includes: A fixed camera is used to continuously acquire images of dynamic obstacles, and the pixel coordinates of the obstacles in the fixed camera images are extracted. The motion velocity of dynamic obstacles in fixed camera coordinates is estimated online using an extended Kalman filter state estimator. The velocity of the obstacle's center projection point is obtained using coordinate system transformation. The rate of change of the radius of the obstacle image projection circle is input into the inequality constraints to update the values ​​of the inequality constraints; The rate of change of the radius of the projection circle of the obstacle image The calculation formula is as follows:

[0017] In the formula: The focal length of the camera; The true radius of the spherical obstacle; The depth of the center point of the obstacle image projection circle; This is the rotation matrix for transforming the end-effector camera coordinate system to the robot arm base coordinate system. This is an estimate of the linear velocity of the obstacle in the base coordinate system.

[0018] As a further improvement of the present invention, the process of S5 includes: The expression for the quadratic programming problem is as follows:

[0019]

[0020] In the formula: The nominal joint angular velocity vector obtained from the nominal controller; By solving a quadratic programming problem, the final optimal joint angular velocity that satisfies the occlusion safety constraints is obtained.

[0021] This invention proposes a visual servo dynamic occlusion avoidance system for robotic arms based on a control obstacle function, comprising: The robotic arm has multiple rotary joints; The visual perception module includes an end-effector camera fixedly mounted at the end of the robotic arm and a fixed camera fixedly mounted above the workspace of the robotic arm. The kinematics establishment module is used to establish a kinematic model of the robotic arm and a vision system model. The vision system model includes an end-effector camera and a fixed camera. Based on the end-effector camera extrinsic parameters and the robotic arm kinematic model, the transformation relationship between the robotic arm and joint motion and the end-effector camera motion speed is constructed. The nominal vision servo controller module is used to set the desired position of the reference feature point for the vision task based on the transformation relationship and the image information of the end camera, and to design the nominal vision servo controller to calculate the nominal joint angular velocity of the robotic arm based on the image error between the current position and the desired position of the reference feature point. The control obstacle function safety constraint module is used to construct safety constraints based on the control obstacle function. It projects dynamic obstacles onto the image plane, constructs a control obstacle function in the image space to quantify the safe distance between the reference feature point and the obstacle projection, and obtains the inequality constraint with respect to the nominal joint angular velocity of the robotic arm by differentiating the control obstacle function. The state observation module is used to observe dynamic obstacles using a fixed camera, estimate the motion state of the obstacles online through an extended Kalman filter state estimator, and input the estimated obstacle velocity information into the inequality constraints. The solution output module is used to construct and solve a quadratic programming problem in each control cycle with the optimization objective of minimizing the difference between the actual control quantity and the nominal joint angular velocity, using inequality constraints as safety conditions, to obtain the optimal joint angular velocity and output it to the robotic arm for execution.

[0022] This invention proposes a robotic arm visual servo dynamic occlusion avoidance device based on a control obstacle function, comprising a processor and a memory, wherein the processor executes a computer program stored in the memory to implement the robotic arm visual servo dynamic occlusion avoidance method based on a control obstacle function as described above.

[0023] This invention proposes a computer-readable storage medium for storing a computer program, wherein the computer program, when executed by a processor, implements the above-described method for dynamic occlusion avoidance of a robotic arm based on a control obstacle function.

[0024] Compared with the prior art, the present invention achieves the following technical effects: This invention directly constructs safety constraints between reference feature points and the projection of dynamic obstacles in the image plane, transforming the visual semantic problem of preventing reference feature points from being occluded into explicit mathematical constraints for the obstacle control function. This approach directly reflects the essential requirements of visual servoing tasks and is more targeted and effective than traditional geometric obstacle avoidance methods. Furthermore, this invention explicitly incorporates the influence of the dynamic obstacle's own motion on the changes in the image occlusion region into the derivative of the obstacle control function. By estimating the dynamic obstacle's motion velocity online using an extended Kalman filter state estimator and compensating it into the inequality constraints, it can continuously constrain reference feature points away from potential occlusion regions, preventing feature point loss, even when the dynamic obstacle's motion state is unknown and dynamically changing.

[0025] This invention utilizes a fixed camera to estimate the motion state of dynamic obstacles online using an extended Kalman filter state estimator, and inputs the estimated obstacle base coordinate system linear velocity into the constraints. This eliminates the need for prior knowledge of the obstacle's motion model or velocity information, significantly improving the method's applicability in unknown and complex environments. In each control cycle, this invention constructs a quadratic programming problem, using the nominal joint angular velocity as the optimization objective and inequality constraints as safety conditions. It solves for the optimal joint angular velocity that satisfies the safety conditions and is closest to the nominal control value. When there are no obstacles or the occlusion risk is low, the safety constraints are relaxed, and the actual control value equals the nominal control value, degenerating into standard visual servo control. When there is an occlusion risk, the control value is adjusted only within the necessary range, thereby minimizing the impact on visual servo convergence performance while ensuring feature point visibility. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the overall process of the present invention; Figure 2 This is a flowchart of the present invention; Figure 3 This is a schematic diagram of the working space of the robotic arm of the present invention; Figure 4 The example shows the trajectory of reference feature points from the end-view camera after incorporating the dynamic occlusion avoidance function of the present invention. Figure 5 This is a trajectory diagram of reference feature points from the end-view camera when the proposed dynamic occlusion avoidance function is not included in the embodiment. Detailed Implementation

[0027] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0028] See Figure 1 and Figure 2 This embodiment proposes a dynamic occlusion avoidance method for robotic arm visual servoing based on a control obstacle function. It is applied to scenarios where dynamic objects occlude reference feature points (the positions of the reference feature points in this embodiment are obtained in real-time by the end-effector camera, and all reference feature point positions in this embodiment are located on the image plane of the end-effector camera) appear in the visual servo control of a robotic arm. The method includes the following steps: S1. Establish a kinematic model of the robotic arm and a vision system model. The vision system model includes an end-effector camera and a fixed camera. Based on the end-effector camera extrinsic parameters and the kinematic model of the robotic arm, construct the transformation relationship between the robotic arm and joint motion and the end-effector camera motion speed. S2. Based on the transformation relationship and the image information of the end camera, set the expected position of the reference feature point for the visual task, design the nominal visual servo controller, and calculate the nominal joint angular velocity of the robotic arm based on the image error between the current position of the reference feature point and the expected position of the reference feature point. S3. Construct safety constraints based on the control obstacle function, project the dynamic obstacle onto the image plane, construct a control obstacle function in the image space to quantify the safe distance between the reference feature point and the obstacle projection, and differentiate the control obstacle function to obtain the inequality constraints on the nominal joint angular velocity of the robotic arm. S4. Observe the dynamic obstacles using a fixed camera, estimate the motion state of the obstacles online using an extended Kalman filter state estimator, and input the estimated obstacle velocity information into the inequality constraint. S5. Within each control cycle, the optimization objective is to minimize the difference between the actual control quantity and the nominal joint angular velocity. An inequality constraint is used as a safety condition to construct and solve a quadratic programming problem, obtain the optimal joint angular velocity, and output it to the robotic arm for execution.

[0029] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments: like Figure 2 As shown, the robotic arm in this embodiment is preferably a six-degree-of-freedom robotic arm. The solid line represents the current position of the robotic arm, and the dashed line represents the target position. The workspace contains an approximately spherical dynamic obstacle and a fixed reference feature point. The state observation is the pixel coordinates of the reference feature point in the camera image. The visual perception part uses two depth cameras, one of which is an end-effector mounted at the end of the robotic arm, denoted as... The other is a fixed camera positioned above the robotic arm's workspace, denoted as... .

[0030] Step 1: Based on the forward kinematics model of the six-DOF robotic arm and the camera calibration data, obtain the end effector camera of the robotic arm. speed Its expression is as follows:

[0031] In the formula: For the Jacobian matrix of the robotic arm, This is the nominal joint angular velocity vector of the robotic arm, which is also the input for visual servo control. Let be the angle of the i-th joint of the robotic arm.

[0032] That is, based on the camera's external parameters and the physical structure of the robotic arm, a relationship is established between the joint motion of the robotic arm and the motion speed of the end-effector camera.

[0033] Step 2: Determine the nominal control quantity of the robotic arm using an image-based visual servoing method. This refers to the nominal joint angular velocity of the robotic arm when it is moved to the target position without considering obstacles. The specific calculation is shown in the following formula:

[0034] In the formula: The current position of the reference feature point in the defined image. To the desired location The error; For designable controllable gain; The Jacobian matrix for the robotic arm; for ease of subsequent description, the image Jacobian matrix will be used. express and The product; The interaction matrix was used to establish the reference feature point position error. derivative With robotic arm end-effector camera speed Relationship , Defined as:

[0035] In the formula: The focal length of the camera; , , These are the position coordinates of the reference feature point in the camera coordinate system.

[0036] Step 3: Considering the scenario where dynamic obstacles occlude the reference feature points in the robotic arm's workspace, construct a safety constraint based on the control obstacle function. Project the dynamic obstacles in the robotic arm's workspace onto the image plane. Construct a control obstacle function in the image space to quantify the safe distance between the reference feature points and the projected center point of the obstacle. The specific calculation is shown in the following formula:

[0037] In the formula: For a moment Real-time observation of pixel coordinates of reference feature points; For a moment The coordinates of the obstacle's projection center; The radius of the obstacle's projection; To allow for a safe distance; This indicates that the feature point is outside the spherical obstacle and is not occluded; when When the feature point is located on the safety boundary; when When a feature point enters the safety boundary, it will be occluded by an obstacle; once occluded, the feature point's position in the camera image will be invisible. Unknown, at this time Unable to define.

[0038] The time derivative of the control barrier function is taken and rearranged to express a form that includes the control quantity. Format:

[0039]

[0040] In the formula: The Jacobian matrix of the feature point image; The Jacobian matrix of the obstacle center point image is calculated in the same way. ; To connect the rate of change of the obstacle's projected radius with the control quantity Jacobian matrix; The velocity at which the center point is projected in the image due to the obstacle's own motion; The rate of change of the radius of the projected circle in the image caused by the movement of the obstacle itself. and The method for obtaining it will be explained in step four.

[0041] The inequality constraints (CBF constraints) of the control barrier function are:

[0042] In the formula: From Mapped to A strictly monotonically increasing continuous function, and , Without loss of generality, it can be said that... Treat it as a constant coefficient for simplification. The CBF constraint defines a forward-invariant set, when the initial... This can guarantee that when hour, This indicates that the reference feature points in the image space are not occluded.

[0043] Step 4: Using a fixed camera Dynamic obstacles are observed, and the state observer estimates the obstacle's position on the camera online. Motion state in the coordinate system. Since both the robotic arm and the obstacle are in motion, the end-effector camera... What is acquired is the relative motion information of the obstacles. The motion of the obstacles themselves is coupled into the relative motion, so a calibrated fixed camera needs to be introduced. To obtain the velocity of the obstacle in the world coordinate system. Define the obstacle's 6-dimensional state variables:

[0044] The observation is the position of the projection point of the obstacle's center in the image. In addition to the obstacle depth information measured by the depth camera, the obstacle's velocity can be estimated using an extended Kalman filter state estimator. By utilizing coordinate system transformation and camera imaging principles, the velocity of the obstacle's center projection point can be further obtained. Rate of change of the radius of the projection circle of the obstacle image This establishes the inequality constraints in step three. The specific calculation formula is as follows:

[0045] In the formula: The focal length of the camera; The true radius of the spherical obstacle; The depth of the center point of the obstacle image projection circle; For camera The rotation matrix for transforming the coordinate system to the robot arm's base coordinate system.

[0046] Step 5: Within each control cycle, with the optimization objective of minimizing the difference between the actual control quantity and the nominal joint angular velocity, and using inequality constraints as safety conditions, construct and solve a quadratic programming optimization problem. The specific calculation is shown in the following formula:

[0047]

[0048] In the formula: The joint angular velocity vector obtained from the nominal controller. To satisfy the control quantity under the CBF constraint, the result of quadratic programming optimization is to directly apply the control quantity under the CBF constraint. As a control variable, it seeks the closest value within a safe region when the CBF constraint is violated. control quantity This reduces the impact on visual servo convergence performance. By solving this optimization problem, the final joint control input that satisfies the occlusion safety constraints is obtained. This achieves the unification of visual servoing tasks and feature point visibility constraints.

[0049] See Figure 3 This is a schematic diagram of the robotic arm's workspace in this embodiment. The reference feature points serve to provide feedback on the robotic arm's end-effector pose. When there are three or more reference feature points, the positional relationship of the reference feature points in the image corresponds to a unique end-effector pose. The desired position of the reference feature points is determined by the target pose of the robotic arm. Then, by controlling the reference feature points in the image plane to move to the desired position, the robotic arm can be simultaneously controlled to move from the current pose to the target pose.

[0050] See Figure 4 This demonstrates the visual servo control effect of the dynamic occlusion avoidance method of the present invention. Figure 4 As can be seen from (a), the reference feature points in the field of view of the end camera are never obscured by dynamic obstacles. Figure 4 (b) is the control barrier function. The curve that changes over time, with its value never being less than 0, indicates that the reference feature point always meets the safety constraints and is not obstructed during the movement of the robotic arm.

[0051] See Figure 5 To avoid incorporating the visual servo control effect of the dynamic occlusion avoidance method of this invention. From Figure 5 As shown in (a), the reference feature point in the field of view of the end camera will now enter the interior of the obstacle. Figure 5 (b) is the obstacle control function in this scenario. The curve showing the change over time, with values ​​less than 0 in the latter half, indicates that the reference feature point was occluded by an obstacle during that period, and the visual servoing system could not continue operating. This embodiment demonstrates... Figure 4 and Figure 5 The comparison demonstrates that the method of the present invention can effectively ensure the feasibility of the visual servoing scheme in scenarios with dynamic obstacles.

[0052] Based on the same inventive concept, this invention also provides a robotic arm visual servo dynamic occlusion avoidance system based on a control obstacle function. Since the principle of this robotic arm visual servo dynamic occlusion avoidance system based on a control obstacle function is similar to that of the aforementioned robotic arm visual servo dynamic occlusion avoidance method based on a control obstacle function, the implementation of this robotic arm visual servo dynamic occlusion avoidance system based on a control obstacle function can refer to the implementation of the robotic arm visual servo dynamic occlusion avoidance method based on a control obstacle function, and the repeated parts will not be described again.

[0053] In specific implementation, the robotic arm vision servo dynamic occlusion avoidance system based on a control obstacle function provided in this embodiment of the invention specifically includes: The robotic arm has multiple rotary joints; The visual perception module includes an end-effector camera fixedly mounted at the end of the robotic arm and a fixed camera fixedly mounted above the workspace of the robotic arm. The kinematics establishment module is used to establish a kinematic model of the robotic arm and a vision system model. The vision system model includes an end-effector camera and a fixed camera. Based on the end-effector camera extrinsic parameters and the robotic arm kinematic model, the transformation relationship between the robotic arm and joint motion and the end-effector camera motion speed is constructed. The nominal vision servo controller module is used to set the desired position of the reference feature point for the vision task based on the transformation relationship and the image information of the end camera, and to design the nominal vision servo controller to calculate the nominal joint angular velocity of the robotic arm based on the image error between the current position and the desired position of the reference feature point. The control obstacle function safety constraint module is used to construct safety constraints based on the control obstacle function. It projects dynamic obstacles onto the image plane, constructs a control obstacle function in the image space to quantify the safe distance between the reference feature point and the obstacle projection, and obtains the inequality constraint with respect to the angular velocity of the robotic arm joint by differentiating the control obstacle function. The state observation module is used to observe dynamic obstacles using a fixed camera, estimate the motion state of the obstacles online through an extended Kalman filter state estimator, and input the estimated obstacle velocity information into the inequality constraints. The solution output module is used to construct and solve a quadratic programming problem in each control cycle with the optimization objective of minimizing the difference between the actual control quantity and the nominal joint angular velocity, using inequality constraints as safety conditions, to obtain the optimal joint angular velocity and output it to the robotic arm for execution.

[0054] Accordingly, this embodiment of the invention also provides a robotic arm visual servo dynamic occlusion avoidance device based on a control obstacle function, including a processor and a memory, wherein the processor executes a computer program stored in the memory to implement the robotic arm visual servo dynamic occlusion avoidance method based on a control obstacle function provided in this embodiment of the invention.

[0055] For more detailed information on the above methods, please refer to the relevant content disclosed in the foregoing embodiments, which will not be repeated here.

[0056] Accordingly, embodiments of the present invention also provide a computer-readable storage medium for storing a computer program, wherein the computer program, when executed by a processor, implements the above-described robotic arm visual servo dynamic occlusion avoidance method based on a control obstacle function provided in embodiments of the present invention.

[0057] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems, devices, and storage media disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0058] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0059] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented directly by hardware, a software module executed by a processor, or a combination of both. The software module can be located in random access memory (RAM), main memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art.

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

[0061] The foregoing has provided a detailed description of the robotic arm visual servo dynamic occlusion avoidance method, system, device, and storage medium based on the control obstacle function provided by this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A control-obstacle-function-based dynamic occlusion avoidance method for visual servoing of a robot arm, characterized in that, This method is applied to scenarios where reference feature points are dynamically occluded within the field of view of the end-effector camera in the visual servo control of a robotic arm, and includes the following steps: S1. Establish a kinematic model of the robotic arm and a vision system model. The vision system model includes an end-effector camera and a fixed camera. Based on the end-effector camera extrinsic parameters and the kinematic model of the robotic arm, construct the transformation relationship between the robotic arm and joint motion and the end-effector camera motion speed. S2. Based on the transformation relationship and the image information of the end camera, set the expected position of the reference feature point for the visual task, design the nominal visual servo controller, and calculate the nominal joint angular velocity of the robotic arm based on the image error between the current position of the reference feature point and the expected position of the reference feature point. S3. Construct safety constraints based on the control obstacle function, project the dynamic obstacle onto the image plane, construct a control obstacle function in the image space to quantify the safe distance between the reference feature point and the obstacle projection, and differentiate the control obstacle function to obtain the inequality constraints on the nominal joint angular velocity of the robotic arm. S4. Observe the dynamic obstacles using a fixed camera, estimate the motion state of the obstacles online using an extended Kalman filter state estimator, and input the estimated obstacle velocity information into the inequality constraint. S5. Within each control cycle, the optimization objective is to minimize the difference between the actual control quantity and the nominal joint angular velocity. An inequality constraint is used as a safety condition to construct and solve a quadratic programming problem, obtain the optimal joint angular velocity, and output it to the robotic arm for execution.

2. The method of claim 1, wherein, The process S1 includes: A kinematic model of a robotic arm with multiple rotary joints is established, and the pose of the robotic arm end effector in the base coordinate system is described by the joint angle vectors. The end-effector camera is fixedly mounted on the end of the robotic arm, and the transformation matrix from the end-effector camera coordinate system to the end-effector coordinate system is obtained through hand-eye calibration. The fixed camera is fixedly installed above the workspace of the robotic arm, and the transformation matrix from the fixed camera coordinate system to the base coordinate system of the robotic arm is calibrated. Extract at least one reference feature point from the image plane of the end camera, and define the expected pixel coordinates and real-time observed pixel coordinates of the reference feature point on the image plane.

3. The method of claim 1, wherein, The process of S2 includes: Based on the intrinsic parameters of the end-effector camera and the image Jacobian matrix, a mapping relationship is established between the real-time observed pixel coordinates of the reference feature points and the angular velocities of the robotic arm joints. Based on the pixel error between the real-time observed pixel coordinates and the desired pixel coordinates, a control law is designed to make the pixel error exponential converge, and the nominal joint angular velocity vector is calculated.

4. The visual servo dynamic occlusion avoidance method for robotic arms based on obstacle control functions according to claim 1, characterized in that, The process S3 includes: Dynamic obstacles in the environment are abstracted as three-dimensional spheres, which have a center position and a radius. The three-dimensional sphere is projected onto the image plane using an end-point camera to obtain the coordinates of the obstacle projection center and the obstacle projection radius; On the image plane, a control obstacle function is constructed for each reference feature point and each obstacle projection region; By taking the time derivative of the control obstacle function, an inequality constraint is obtained regarding the nominal joint angular velocity of the robotic arm.

5. The visual servo dynamic occlusion avoidance method for robotic arms based on obstacle control functions according to claim 4, characterized in that, The expression for the control barrier function is as follows: In the formula: For a moment Real-time observation of pixel coordinates of reference feature points; For a moment The coordinates of the obstacle's projection center; The radius of the obstacle's projection; To allow for a safe distance; This indicates that the feature point is outside the spherical obstacle and is not occluded; when When the feature point is located on the safety boundary; when When the feature point enters the interior of the safety boundary, it will be obscured by the obstacle. When a feature point is obscured by an obstacle, its location in the camera image cannot be seen. Unknown, then Unable to define; The expression for the inequality constraint is as follows: In the formula: From Mapped to A strictly monotonically increasing continuous function, and , , It can be considered a constant; The time derivative of the control barrier function; Let be the nominal joint angular velocity vector of the robotic arm, where For the first robotic arm The angle of each joint; The expression for the time derivative of the control barrier function is as follows: In the formula: The Jacobian matrix of the feature point image; Let be the Jacobian matrix of the obstacle center point image; To connect the rate of change of the obstacle's projected radius with the control quantity The Jacobian matrix; The velocity at which the center point is projected in the image due to the obstacle's own motion; The rate of change of the radius of the projected circle in the image caused by the movement of the obstacle itself.

6. The visual servo dynamic occlusion avoidance method for robotic arms based on obstacle control functions according to claim 1, characterized in that, The process in S4 includes: A fixed camera is used to continuously acquire images of dynamic obstacles, and the pixel coordinates of the obstacles in the fixed camera images are extracted. The motion velocity of dynamic obstacles in fixed camera coordinates is estimated online using an extended Kalman filter state estimator. The velocity of the obstacle's center projection point is obtained using coordinate system transformation. The rate of change of the radius of the obstacle image projection circle is input into the inequality constraints to update the values ​​of the inequality constraints; The rate of change of the radius of the projection circle of the obstacle image The calculation formula is as follows: In the formula: The focal length of the camera; The true radius of the spherical obstacle; The depth of the center point of the obstacle image projection circle; This is the rotation matrix for transforming the end-effector camera coordinate system to the robot arm base coordinate system. This is an estimate of the linear velocity of the obstacle in the base coordinate system.

7. The visual servo dynamic occlusion avoidance method for robotic arms based on obstacle control functions according to claim 1, characterized in that, The process of S5 includes: The expression for the quadratic programming problem is as follows: In the formula: The nominal joint angular velocity vector obtained from the nominal controller; By solving a quadratic programming problem, the final optimal joint angular velocity that satisfies the occlusion safety constraints is obtained.

8. A visual servo dynamic occlusion avoidance system for a robotic arm based on a control obstacle function, characterized in that, include: The robotic arm has multiple rotary joints; The visual perception module includes an end-effector camera fixedly mounted at the end of the robotic arm and a fixed camera fixedly mounted above the workspace of the robotic arm. The kinematics establishment module is used to establish a kinematic model of the robotic arm and a vision system model. The vision system model includes an end-effector camera and a fixed camera. Based on the end-effector camera extrinsic parameters and the robotic arm kinematic model, the transformation relationship between the robotic arm and joint motion and the end-effector camera motion speed is constructed. The nominal vision servo controller module is used to set the desired position of the reference feature point for the vision task based on the transformation relationship and the image information of the end camera, and to design the nominal vision servo controller to calculate the nominal joint angular velocity of the robotic arm based on the image error between the current position and the desired position of the reference feature point. The control obstacle function safety constraint module is used to construct safety constraints based on the control obstacle function. It projects dynamic obstacles onto the image plane, constructs a control obstacle function in the image space to quantify the safe distance between the reference feature point and the obstacle projection, and obtains the inequality constraint with respect to the nominal joint angular velocity of the robotic arm by differentiating the control obstacle function. The state observation module is used to observe dynamic obstacles using a fixed camera, estimate the motion state of the obstacles online through an extended Kalman filter state estimator, and input the estimated obstacle velocity information into the inequality constraints. The solution output module is used to construct and solve a quadratic programming problem in each control cycle with the optimization objective of minimizing the difference between the actual control quantity and the nominal joint angular velocity, using inequality constraints as safety conditions, to obtain the optimal joint angular velocity and output it to the robotic arm for execution.

9. A visual servo dynamic occlusion avoidance device for a robotic arm based on a control obstacle function, characterized in that, It includes a processor and a memory, wherein the processor executes a computer program stored in the memory to implement the visual servo dynamic occlusion avoidance method for a robotic arm based on a control obstacle function as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, Used to store a computer program, wherein the computer program, when executed by a processor, implements the visual servo dynamic occlusion avoidance method for a robotic arm based on a control obstacle function as described in any one of claims 1 to 7.

Images