A visual servo and safe obstacle avoidance method for a robotic arm based on disturbance observer

Abstract

The present invention discloses a visual servoing and safe obstacle avoidance method for a robot based on a disturbance observer, and belongs to the field of visual servoing control of robots. A second-order model of image-based visual servoing and a constraint on the visibility of feature objects are established; the envelope simplification of the robot arm and the establishment of robot-related safety constraints are completed; the difficult-to-calculate items in the visual servoing model are regarded as uncertainties, and an extended state observer is designed to observe the pixel speed and uncertainty; in an ideal case, a CBF constraint is established according to the safety distance constraint, angular velocity and angular acceleration constraint, and then a CBF considering uncertainty is established; finally, a reference controller considering uncertainty, a CLF based on a preset time and an optimization solution objective function are designed to solve the safety optimization problem. This method is efficient and stable, and can not only ensure that the robot arm can quickly estimate the uncertainty, but also realize that the estimated value of the uncertainty is obtained and applied to the safety constraint.

Description

Technical Field

[0001] The invention relates to a mechanical arm visual servo and safety obstacle avoidance method based on a disturbance observer, and belongs to the field of visual servo control of robots. Background Art

[0002] In the field of robot control, various visual servo robots based on visual feedback have emerged one after another, playing an important role in industry, military, civilian and scientific exploration. In some fields, visual servo control schemes have received more attention, especially in some scenarios with high control accuracy requirements.

[0003] Compared with other feedback methods, visual feedback is easier to obtain, but more difficult to control: on the one hand, as a part of the feedback, the image quality of the camera is affected by factors such as lens quality, photosensitive element quality, and photosensitive element size, which directly affects the results of subsequent image processing and feature extraction; on the other hand, there are many unsafe factors in the actual experimental environment, including but not limited to loss of camera field of view, collision between the robot end and obstacles, collision between the robot body and obstacles, etc., so it is necessary to model these unsafe factors and use optimization methods to optimize the reference control quantity to obtain a control quantity that meets safety requirements; due to the limitations of actual experimental conditions, the elements in the mathematical model established in the early stage may not be able to be obtained in real time or even cannot be obtained, so it is necessary to estimate the uncertain items and apply the estimated results to the control strategy.

[0004] In terms of traditional control algorithms, in order to ensure the safety of visual servo-based robot operation, neural networks, preset performance and other methods are often used to meet control requirements. Although this type of control algorithm is simple and convenient to use, the above algorithms have problems such as long preparation time, difficulty in expansion, inability to constrain control quantities, and failure to consider internal and external uncertainties.

[0005] Therefore, in the control process, there are two urgent problems that need to be solved: on the one hand, how to effectively estimate the uncertainty of system modeling; on the other hand, how to ensure that after the estimated value of uncertainty is obtained, it is applied to the safety constraint to improve its stability and safety.

[0006] Therefore, in the control process, on the one hand, how to effectively estimate the uncertainty of system modeling so that the error between the estimated result and the actual value converges quickly; on the other hand, how to ensure that the estimated value of the uncertainty is applied to the safety constraint after obtaining it to improve its stability and safety, both are urgent problems that need to be solved by the current multi-robot algorithm. Summary of the invention

[0007] Purpose of the invention: In view of the problems and shortcomings of the above-mentioned prior art, the present invention aims to propose a visual servoing and safe obstacle avoidance method for a robotic arm based on a disturbance observer. By combining an expanded state observer with a CLF+CBF optimization control strategy, it can not only ensure that the IBVS-based robot system can quickly estimate the uncertainty, but also apply the estimated value of the uncertainty to the safety constraint after obtaining it, thereby improving its stability and safety, and finally completing the visual servo control task.

[0008] Technical solution of the present invention: In order to achieve the above-mentioned invention object, the present invention discloses a visual servoing and safe obstacle avoidance method of a manipulator based on a disturbance observer, a camera is set at the end of the manipulator, and the coordinate systems of the camera and the end of the manipulator coincide, and the steps are as follows:

[0009] Step 1: Establish a camera imaging model, use the camera imaging model to generate an image-based second-order IBVS visual servo model, set non-collinear feature points greater than or equal to 4 in the workspace, and the robot arm uses the camera to shoot and feedback images containing feature points to determine the position of the end of the robot arm in real time. During the movement of the robot arm, it is necessary to ensure that the feature points always remain within the field of view of the camera;

[0010] Step 2: Envelope and simplify the robot arm, regard the joints and connecting rods of the robot arm as spheres and cylinders of corresponding radii respectively, ignore the support terminal, set the safety distance constraints of the joints and connecting rods to obstacles, and the angular velocity and angular acceleration constraints of the robot arm joints, so as to ensure that each joint and connecting rod of the robot arm does not collide with obstacles;

[0011] In the visual servo task, the moving target of the robot arm is simplified to the pixel position. The end of the robot arm judges its own position through the pixel position of the feature point fed back by the camera and moves until the pixel position of the feature point reaches the expected position. At this time, the robot arm also reaches the predetermined posture. In this process, the safety distance between each joint and connecting rod and the obstacle is guaranteed to be greater than zero, and the angular velocity and angular acceleration of the robot arm during operation are limited not to exceed the safe range of the robot arm's load handling, thereby ensuring the safety of the robot arm's operation.

[0012] Step 3: Since the second-order IBVS visual servo model contains strong nonlinear terms observed by the observer, and the strong nonlinear terms are uncertain terms, an extended state observer is used to observe the pixel velocity and uncertainty;

[0013] Step 4: Ideally, a high-order control obstacle function (CBF) constraint is established based on the safety distance constraint, angular velocity and angular acceleration constraints established in step 2. Since there is a slight deviation between the observation result of the observer and the actual value, the accuracy requirement of the CBF constraint cannot be met. Therefore, a CBF that takes uncertainty into account is established.

[0014] Step 5, design a reference controller that takes uncertainty into account, and solve the obstacle avoidance safety optimization problem of the robotic arm based on the preset time Lyapunov function CLF and the optimization objective function.

[0015] Compared with the prior art, the present invention provides a visual servoing and safe obstacle avoidance method for a manipulator based on a disturbance observer, and its beneficial effects include:

[0016] 1) In terms of ensuring the stability of the IBVS-based robot, the present invention firstly adopts a proportional differential controller to generate a reference control quantity, which can ensure that the system is stable without considering safety.

[0017] 2) In dealing with modeling uncertainty, the present invention adopts a high-order extended state observer to observe it, and the observation results show that it can track the target signal well.

[0018] 3) In terms of ensuring the safety of the system, the present invention divides safety into four aspects: visibility of the field of view, obstacle avoidance between joints and obstacles, obstacle avoidance between each joint and obstacles, and constraining the angular velocity and angular acceleration of the robot joints. In the mathematical model established above, only the visual servoing part has uncertainties. In the form of the given extended observer, the uncertainties and other state quantities are observed, and the observation results are used in the construction of safety constraints. The angular velocity and angular acceleration of the robot joints ensure the feasibility of the robot's response to the input signal and the safety protection of the robot itself. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a flow chart of the visual servoing and safe obstacle avoidance method of a manipulator based on a disturbance observer of the present invention;

[0020] Figure 2 A schematic diagram of a robot workspace based on IBVS used in an embodiment of the present invention;

[0021] Figure 3 It is a simplified schematic diagram of the structure envelope of the mechanical arm in an embodiment of the present invention;

[0022] Figure 4 It is a schematic diagram of an IBVS controller based on optimization in an embodiment of the present invention;

[0023] Figure 5 It is a block diagram of the optimized IBVS control algorithm in the embodiment of the present invention;

[0024] Figure 6 Schematic diagram of movement trajectories of four feature points in an embodiment of the present invention;

[0025] Figure 7 Schematic diagram of the change of pixel error convergence of four feature points in an embodiment of the present invention;

[0026] Figure 8 Schematic diagram of the changes of CLF and CBF in an embodiment of the present invention;

[0027] Fig. 9 Schematic diagram of the change of joint angular velocity of an 8-joint robotic arm in an embodiment of the present invention;

[0028] Fig.10 Schematic diagram of the change of joint angular acceleration of an 8-joint robotic arm in an embodiment of the present invention;

[0029] Fig.11 Schematic diagram of estimation results of uncertain items in an embodiment of the present invention; DETAILED DESCRIPTION

[0030] Combine the following Figure 1 The embodiments of the present invention are explained in more detail.

[0031] like Figure 1 As shown, the present invention discloses a visual servoing and safe obstacle avoidance method for a robotic arm based on a disturbance observer, and the steps are as follows:

[0032] Step 1, establishing a second-order model of image-based visual servoing and constraints on feature visibility;

[0033] Use the camera to establish a camera imaging model: Use the camera to obtain the pixel coordinates of the feature point on the image plane. For the i-th feature point, convert the point [X wi ,Y wi ,Z wi ]First, transform it into the point [X Ci ,T Ci ,Z Ci ], and finally converted into pixel coordinates [x i ,y i ] T , the transformation relationship is:

[0034]

[0035] Where, d x With d y are the size of a single pixel, f is the focal length of the camera, u0 and v0 are the horizontal and vertical coordinate components of the camera principal point coordinates, K is the camera's intrinsic parameter matrix, R is the rotation matrix from the camera coordinate system to the world coordinate system, and t is the translation vector from the camera coordinate system to the world coordinate system;

[0036] By taking the derivative of both ends of formula (1), we get formula (2):

[0037]

[0038] Substitute equation (2) into the relationship between the angular velocity of the point in the three-dimensional space and the linear velocity and angular velocity of the coordinate system: v x 、v y 、v z and ω x ,ω y ,ω z They are the linear velocity and angular velocity of the camera in its own coordinate system, respectively, and s = [xy] T Represent the pixel coordinates of a feature point on the imaging plane and differentiate it to obtain the first-order IBVS visual servo model:

[0039]

[0040] Where, L i Represents the image Jacobian matrix of the i-th feature point, Represents the camera speed in its own coordinate system. Derivative (3) gives the second-order IBVS visual servoing model:

[0041]

[0042] In the formula, assume

[0043]

[0044]

[0045] Ω xi and Ω yi All represent speed-related coefficient matrices; the present invention selects a total of four feature points, and the pixel vectors of the feature points are recorded as According to the analysis of formula (3)-(4), we have

[0046]

[0047] (5)-(6) Where L = [L1; L2; L3; L4], L v =[L v1 ; L v2 ; L v3 ; L v4 ].

[0048] Mapping the motion of pixels to the motion of the robot joints The second-order IBVS visual servo model expressed in the end coordinate system of the robotic arm actuator is obtained as follows:

[0049]

[0050] Where J1 is the Jacobian matrix of the robot in the end coordinate system, is the acceleration of the end of the robot in its own coordinate system, q, are the angle, angular velocity, and angular acceleration of the robot joint, respectively; u is the joint angular acceleration input of the robot; and according to the conversion relationship between the robot end coordinate system and the robot base coordinate system, formula (5) is converted into the representation in the base coordinate system:

[0051]

[0052] In the formula, is the rotation matrix from the robot end coordinate system to the base coordinate system, and J0 is the Jacobian matrix of the robot in the base coordinate system;

[0053] In the visual servoing task, the pixel position of the feature point is controlled by the control algorithm to move to the desired pixel position, so the feature point needs to always remain within the camera's field of view; the camera's rectangular field of view is described by the maximum and minimum values ​​of the pixel: max 、x min ,y max ,y min To ensure the visibility of feature point s, the following constraints need to be always met:

[0054]

[0055] In the formula, represents the dot product of vectors, s=[x min y min x min y min x min y min x min y min ] T .h1 is used to indicate that in the current state, the farther the feature point value is from 0, the closer it is to the center of the field of view; if the feature point is at the boundary of the field of view, the value is 0, and outside the field of view, the value is <0;

[0056] Step 2, complete the envelope simplification of the robot and establish the safety constraints related to the robot to ensure that each joint and link of the robot does not collide with obstacles; by ensuring the distance between each joint and link to the obstacle, and limiting the angular velocity and angular acceleration of the robot during operation will not exceed the safe range of the load, to ensure the safety of the robot operation: the envelope simplification of the robot means that its joints and links are regarded as spheres and cylinders of corresponding radii respectively, and the safety constraints include the safety distance constraints from the joints and links to the obstacles, and the angular velocity and angular acceleration constraints of the robot joints, such as Figure 2 and Figure 3 As shown;

[0057] Simplify the revolute joint to a position that coincides with the revolute joint coordinate system. Only the safety issues of the revolute joint, the end, and the connecting rod need to be considered. Suppose the revolute joint and the end are enclosed by a radius of The radius of the spherical obstacle is R o , the joint and the spherical obstacle P o The distance between Similarly, by introducing the distance formula from a point in space to a straight line passing through two fixed points, we can express the radius R o The obstacle P o Been to the joint And the radius is The distance between the connecting rods is To ensure the safety of the robot arm, the distance between each joint and link of the robot arm and the obstacle must be strictly greater than zero, that is, the constraint h should be satisfied. 2i >0,h 3i >0;

[0058] Assume that the constraints of angular velocity and angular acceleration are The corresponding constraint expression is in represents the dot multiplication of vectors; then all constraints to ensure the safety of the robot arm in avoiding obstacles are:

[0059]

[0060] In the formula, h 21 -h 22 is the distance between the joint and the obstacle, h 31 -h 34 is the distance between each link and the obstacle, h4 and h5 are the distance values ​​between the robot arm joint angular velocity and joint angular acceleration and their respective constraint boundaries.

[0061] Step 3, regard the difficult-to-calculate items in the visual servo model established in step 1 as uncertainties, and design an extended state observer to observe the pixel velocity and uncertainty;

[0062] Let state X1 = s, X3=q, The IBVS visual servo kinematic model (6) is written in the form of a state space equation:

[0063]

[0064] In the formula, is the Jacobian matrix of the robot in the end coordinate system, is the rotation matrix from the robot end coordinate system to the base coordinate system, and u is the acceleration controller;

[0065] because There is uncertainty in the term Therefore, it is considered as an uncertain term and an extended state observer with a preset time is used to observe it; the extended state observer with a preset time is as follows:

[0066]

[0067] In the formula, are the estimated values ​​of states X1, X2, X3, and X4, is the estimated value of uncertainty B1, ρ1, ρ2, ρ3, ρ4, ρ5 are constants greater than zero, To ensure that the observer converges within the preset time, ρ6>0, ρ7>1, l>2, are some constants, T f >0 is a custom preset time.

[0068] Step 4, firstly, establish the CBF constraints according to the safety distance constraints, angular velocity and angular acceleration constraints established in step 2 under ideal conditions, and then establish the CBF considering the uncertainty;

[0069] The safety distance constraints from the robot joint, the end point of the robot and the connecting rod to the obstacle are transformed into the constraint form of CBF optimization solution; among them, the angular acceleration constraint is directly applied to the control quantity. Constraint is performed, and the relative degree of this constraint is 0; the angular velocity constraint is the control quantity The integral of the constraint is constrained, and the relative degree of this constraint is 1; the visibility of feature points, obstacle avoidance of robot joints, and obstacle avoidance constraints of robot links are all constraints on the control quantity The second-order integral of is constrained, and the relative degree of this constraint is 2; therefore, the first-order and second-order CBFs are applied to the above cases respectively:

[0070] For the angular acceleration constraint, the control quantity is directly Add constraints

[0071] For the angular velocity constraint h4 ≥ 0, use the previously constructed CBF manipulator joint angular velocity h4 and first calculate the first-order derivative of h4 Then it can be rewritten as the first-order CBF form L that satisfies uncertainty. f h+L g The constraint equation of hu≥-α(h):

[0072]

[0073] For the feature point visibility constraint, first calculate the first and second order derivatives of h1:

[0074]

[0075] Then rewrite it into a second-order CBF form that satisfies uncertainty The constraint equations are:

[0076]

[0077] α 11 , α 12 It is a K-type function. A K-type function refers to a continuous function that is strictly increasing in the interval [0, a) and takes the value of 0 at the origin. Here, it is replaced by a constant greater than zero;

[0078] because The uncertainty is included Therefore, the observation results of the extended state observer constructed in step 3 are used to replace the uncertain terms to obtain Modify the reference input to the observed value of the uncertainty term B1 After the value is expressed, the IBVS visual visibility constraint needs to be modified to the following form:

[0079]

[0080] M be is a greater than The constant of the upper bound;

[0081] For joint obstacle avoidance, use the CBFh constructed previously 2i , first find h 2i The first and second derivatives of :

[0082]

[0083] Then rewrite it to satisfy the second-order CBF form The constraint equations are:

[0084]

[0085] In the formula, α 21 , α 22 It is a K-type function, and is replaced here by a constant greater than zero.

[0086] For link obstacle avoidance, use the CBFh constructed previously 3i , first find h 3i The first and second derivatives of :

[0087]

[0088] Rewrite the above formula into the second-order CBF form The constraint equations are:

[0089]

[0090] In the formula, α 31 , α 32 It is a K-type function, and is replaced here by a constant greater than zero.

[0091] Step 5, design a reference controller that takes uncertainty into account, a CLF based on a preset time, and an optimization objective function to solve the safety optimization problem, such as Figure 4 and Figure 5 As shown;

[0092] The error in the visual servoing task is given by the expected pixel s * and the current pixel s generates an error value e=ss * , design a proportional-derivative controller as the reference controller

[0093]

[0094] In order to ensure that the optimized solution can ensure system stability, it is necessary to add CLF constraints to the CBF constraints obtained in step 4, considering the CLF of the preset time: where e = ss * , c1 is a coefficient greater than 0, s * For the desired pixel position, the first-order differentiation of CLF is obtained

[0095]

[0096] Since e=ss * Medium * is a constant, then Through the second-order kinematics of IBVS and the second-order kinematics of the robot, we have

[0097]

[0098] Therefore, according to the definition of CLF and replacing the uncertain terms, a CLF form that satisfies the uncertainty consideration is constructed. The constraint equations are:

[0099]

[0100] l1 and l2 are constants greater than 0 and 1 respectively. To ensure that the CLF converges within the specified time, m>2, b is a constant greater than zero, and M be is a greater than The constant of the upper bound; λ and μ are the terms that constitute the preset time CLF, which can ensure that the CLF is within the preset time T p convergence;

[0101] The optimization objective function is designed as u n is the reference constraint without considering the safety issue, and u is the optimization result after considering the safety issue. In order to ensure the optimal energy, the optimization result u will not disappear or become very large;

[0102] In order to avoid the optimization problem from being unsolvable when the safety constraints are strict, a slack variable δ is added after the CLF constraint to temporarily relax the convergence condition. Then all safety constraint problems: CLF constraint, feature point visibility constraint, robot joint obstacle avoidance constraint, robot link obstacle avoidance constraint, robot joint angular velocity constraint, robot angular acceleration constraint are transformed into optimization problems:

[0103]

[0104] In one embodiment of the present invention, Figure 6 As described above, there are four feature points S1-S4 in the workspace of the second-order IBVS visual servoing model. During the movement of the robot arm, the moving trajectories of the four feature points are recorded using four different colors. In the figure, "Δ" represents the starting point of the feature point trajectory, and "*" represents the expected point of the feature point trajectory;

[0105] like Figure 7 As shown in the figure, the change of the convergence of the pixel error of the feature points, the four feature point trajectories formed by the four feature points, each trajectory needs to be described by two sets of data, x and y, and the pixel errors of the four feature points form 8 curves, which are described by different colors;

[0106] Figure 8 The changes of CLF and CBF in the embodiment of the present invention are shown in FIG. Figure 8The upper middle sub-figure is a schematic diagram of the activation of CLF. Each curve in the figure represents the error size of the x and y dimensions of the four feature points; Figure 8 The middle sub-graph is the activation status of CBF to ensure the visibility of feature points, indicating whether the four feature points are always within the camera's field of view during the operation of the robot. The results show that all curve values ​​of the second sub-graph are greater than zero, that is, all feature points are always within the camera's range. Each feature point has two dimensions, x and y, so 8 data are needed to describe the positions of the four feature points. Different feature points and the x and y dimensional error data of the same feature point have been described using different colors, and the color distinction is reflected in the legend; Figure 8 The sub-graph in the middle and lower part shows the distance between the three movable links of the robot and the obstacle during the simulation using three lines of different colors. The results show that the distance between the three movable links of the robot and the obstacle is always greater than zero during the operation, that is, the robot is always in a safe state.

[0107] Fig. 9 The figure shows the change of the angular velocity of the robot arm joints in the embodiment. Assume that the robot arm has a total of 7 joints, and the angular velocity of each joint runs within the set angular velocity range of ±0.15 radians per second. Different joint angular velocities are represented by curves of different colors.

[0108] Fig.10 The figure shows the change of angular acceleration of the robot arm joints in the embodiment. Assume that the robot arm has a total of 7 joints, and the angular acceleration of each joint runs within the set angular velocity range of ±0.15 radians per second. Different joint angular velocities are represented by curves of different colors.

[0109] Fig.11 This is the estimation result of the uncertainty term in the embodiment. Since the uncertainty term is a part of the pixel acceleration of the feature point, in the model space of the four feature points, the uncertainty term is understood as the uncertainty value of the acceleration of each feature point in the x and y dimensions, which is represented by 8 curves of different colors.

Claims

1. A visual servoing and safe obstacle avoidance method for a manipulator based on a disturbance observer, characterized in that: A camera is set at the end of the robotic arm, and the coordinate systems of the camera and the end of the robotic arm coincide. The steps are as follows: Step 1: Establish a camera imaging model, use the camera imaging model to generate an image-based second-order IBVS visual servo model, set non-collinear feature points greater than or equal to 4 in the workspace, and the robot arm uses the camera to shoot and feedback images containing feature points to determine the position of the end of the robot arm in real time. During the movement of the robot arm, it is necessary to ensure that the feature points always remain within the field of view of the camera; Step 2: Envelope and simplify the robot arm, regard the joints and connecting rods of the robot arm as spheres and cylinders of corresponding radii respectively, ignore the clamping terminal, set the safety distance constraints from the joints and connecting rods to the obstacles, and the angular velocity and angular acceleration constraints of the robot arm joints, so as to ensure that each joint and connecting rod of the robot arm does not collide with the obstacles; In the visual servo task, the moving target of the robot arm is simplified to the pixel position. The end of the robot arm judges its own position through the pixel position of the feature point fed back by the camera and moves until the pixel position of the feature point reaches the expected position. At this time, the robot arm also reaches the predetermined posture. In this process, the safety distance between each joint and connecting rod and the obstacle is guaranteed to be greater than zero, and the angular velocity and angular acceleration of the robot arm during operation are limited not to exceed the safe range of the robot arm's load handling, thereby ensuring the safety of the robot arm's operation. Step 3: Since the second-order IBVS visual servo model contains strong nonlinear terms observed by the observer, and the strong nonlinear terms are uncertain terms, an extended state observer is used to observe the pixel velocity and uncertainty; Step 4: Ideally, a high-order control obstacle function (CBF) constraint is established based on the safety distance constraint, angular velocity and angular acceleration constraints established in step 2. Since there is a slight deviation between the observation result of the observer and the actual value, the accuracy requirement of the CBF constraint cannot be met. Therefore, a CBF that takes uncertainty into account is established. Step 5, design a reference controller that takes uncertainty into account, and solve the obstacle avoidance safety optimization problem of the robotic arm based on the preset time Lyapunov function CLF and the optimization objective function.

2. According to claim 1, a method for visual servoing and safe obstacle avoidance of a manipulator based on a disturbance observer is characterized in that In step 1, a second-order model based on image visual servoing and field of view constraints are established: Use the camera to establish a camera imaging model: Use the camera to obtain the pixel coordinates of the feature point on the image plane. For the i-th feature point, convert the point [X wi ,Y wi ,Z wi ]First, transform it into the point [X Ci ,Y Ci ,Z Ci ], and finally converted into pixel coordinates [x i ,y i ] T , the transformation relationship is: Where, d x With d y are the size of a single pixel, f is the focal length of the camera, u0 and v0 are the horizontal and vertical coordinate components of the camera principal point coordinates, K is the camera's intrinsic parameter matrix, R is the rotation matrix from the camera coordinate system to the world coordinate system, and t is the translation vector from the camera coordinate system to the world coordinate system; By taking the derivative of both ends of formula (1), we get formula (2): Substitute equation (2) into the relationship between the angular velocity of the point in the three-dimensional space and the linear velocity and angular velocity of the coordinate system: v x 、v y 、v z and ω x ,ω y ,ω z They are the linear velocity and angular velocity of the camera in its own coordinate system, respectively, and s = [xy] T Represent the pixel coordinates of a feature point on the imaging plane and differentiate it to obtain the first-order IBVS visual servo model: Where, L i Represents the image Jacobian matrix of the i-th feature point, Represents the camera speed in its own coordinate system; deriving equation (3), we get the second-order IBVS visual servoing model: In the formula, assume Ω xi and Ω yi All represent the coefficient matrix related to speed; a total of four feature points are selected, and the pixel vector of the feature point is recorded as According to the analysis of formula (3)-(4), we have (5)-(6) Where L = [L1; L2; L3; L4], L v =[L v1 ; L v2 ; L v3 ; L v4 ]; Mapping the motion of pixels to the motion of the robot joints The second-order IBVS visual servo model expressed in the end coordinate system of the robotic arm actuator is obtained as follows: Where J1 is the Jacobian matrix of the robot in the end coordinate system, is the acceleration of the end of the robot arm in its own coordinate system, are the angle, angular velocity, and angular acceleration of the robot joint, respectively; u is the joint angular acceleration input of the robot; and according to the conversion relationship between the robot end coordinate system and the robot base coordinate system, formula (5) is converted into the representation in the base coordinate system: In the formula, is the rotation matrix from the robot end coordinate system to the base coordinate system, and J0 is the Jacobian matrix of the robot in the base coordinate system; In the task of the second-order IBVS visual servoing model, the pixel position of the feature point is controlled by the control algorithm to move to the desired pixel position, so the feature point needs to always remain within the camera's field of view; the camera's rectangular field of view is described by the maximum and minimum values ​​of the pixel: x max 、x min ,y max ,y min To ensure the visibility of feature point s, the following constraints need to be always established: In the formula, represents the dot product of vectors, s =[x min y min x min y min x min y min x min y min ] T , h1 is used to indicate that in the current state, the farther the feature point value is from 0, the closer it is to the center of the field of view; if the feature point is at the boundary of the field of view, the value is 0, and outside the field of view, the value is <0.

3. According to claim 2, a method for visual servoing and safe obstacle avoidance of a manipulator based on a disturbance observer is characterized in that In step 2, the robot arm is simplified by enveloping, and each link of the robot arm and its two endpoints are regarded as a cylinder and a sphere of corresponding radius respectively, and the safety distance constraints from the joints and links to obstacles, the angular velocity and angular acceleration constraints of the robot arm joints are set respectively, so as to ensure that each joint and link of the robot arm does not collide with obstacles; The safety of the robot operation is ensured by ensuring that the distance between each joint and link and the obstacle is not less than 100%, and limiting the angular velocity and angular acceleration of the robot during operation so that they do not exceed the safe range of the load: Simplify the revolute joint to a position that coincides with the revolute joint coordinate system. Only the safety issues of the revolute joint, the end, and the connecting rod need to be considered. Suppose the revolute joint and the end are enclosed by a radius of The radius of the spherical obstacle is R o , the joint and the spherical obstacle P o The distance between Similarly, by introducing the distance formula from a point in space to a straight line passing through two fixed points, we can express the radius R o The obstacle P o Been to the joint And the radius is The distance between the connecting rods is To ensure the safety of the robot arm, the distance between each joint and link of the robot arm and the obstacle must be strictly greater than zero, that is, the constraint h should be satisfied. 2i >0,h 3i >0; Assume that the constraints of angular velocity and angular acceleration are The corresponding constraint expression is in represents the dot multiplication of vectors; then all constraints to ensure the safety of the robot arm in avoiding obstacles are: In the formula, h 21 -h 22 is the distance between the joint and the obstacle, h 31 -h 34 is the distance between each link and the obstacle, h4 and h5 are the distance values ​​between the robot arm joint angular velocity and joint angular acceleration and their respective constraint boundaries.

4. According to claim 3, a method for visual servoing and safe obstacle avoidance of a manipulator based on a disturbance observer, characterized in that: In step 3, the difficult-to-calculate items in the visual servoing model established in step 1 are regarded as uncertainty items, and an extended state observer is designed to observe the pixel velocity and uncertainty. Set state The second-order IBVS visual servo model (6) is written in the form of a state space equation: In the formula, is the Jacobian matrix of the robot in the end coordinate system, is the rotation matrix from the robot end coordinate system to the base coordinate system, and u is the acceleration controller; because There is uncertainty in the term Therefore, it is considered as an uncertain term and an extended state observer with a preset time is used to observe it; the extended state observer with a preset time is as follows: In the formula, are the estimated values ​​of states X1, X2, X3, and X4, is the estimated value of uncertainty B1, ρ1, ρ2, ρ3, ρ4, ρ5 are constants greater than zero, To ensure that the observer converges within the preset time, ρ6>0, ρ7>1, l>2, all of which are constants, T f >0 is a custom preset time.

5. According to claim 4, a disturbance observer-based manipulator visual servoing and safe obstacle avoidance method is characterized in that In step 4, first, under ideal conditions, the CBF constraints are established according to the safety distance constraints, angular velocity and angular acceleration constraints established in step 2, and then the CBF considering uncertainty is established: The safety distance constraints from the robot joint, the end point of the robot and the connecting rod to the obstacle are transformed into the constraint form of CBF optimization solution; among them, the angular acceleration constraint is directly applied to the control quantity. Constraint is performed, and the relative degree of this constraint is 0; the angular velocity constraint is the control quantity The integral of the constraint is constrained, and the relative degree of this constraint is 1; the visibility of feature points, obstacle avoidance of robot joints, and obstacle avoidance constraints of robot links are all constraints on the control quantity The second-order integral of is constrained, and the relative degree of this constraint is 2; therefore, the first-order and second-order CBFs are applied to the above cases respectively: For the angular acceleration constraint, the control quantity is directly Add constraints For the angular velocity constraint h4 ≥ 0, use the CBF robot arm joint angular velocity h4 and first calculate the first-order derivative of h4 Then it can be rewritten as the first-order CBF form L that satisfies uncertainty. f h+L g The constraint equation of hu≥-α(h): For the feature point visibility constraint, first calculate the first and second order derivatives of h1: Then rewrite it into a second-order CBF form that satisfies uncertainty The constraint equations are: α 11 , α 12 It is a K-type function. A K-type function refers to a continuous function that is strictly increasing in the interval [0, a) and takes the value of 0 at the origin. Here, it is replaced by a constant greater than zero; because The uncertainty is included Therefore, the observation results of the extended state observer constructed in step 3 are used to replace the uncertain terms to obtain Modify the reference input to the observed value of the uncertainty term B1 After the value is expressed, the IBVS visual visibility constraint needs to be modified to the following form: M be is a greater than The constant of the upper bound; For joint obstacle avoidance, use CBFh 2i , first find h 2i The first and second derivatives of : Then rewrite it to satisfy the second-order CBF form The constraint equations are: In the formula, α 21 , α 22 It is a K-type function, which is replaced by a constant greater than zero here; For connecting rod obstacle avoidance, use CBFh 3i , first find h 3i The first and second derivatives of : Rewrite the above formula into the second-order CBF form The constraint equations are: In the formula, α 31 , α 32 It is a K-type function, and is replaced here by a constant greater than zero.

6. According to claim 5, a method for visual servoing and safe obstacle avoidance of a manipulator based on a disturbance observer is characterized in that In step 5, a reference controller considering uncertainty is designed to solve the safety optimization problem based on the CLF of the preset time and the optimization objective function: The error in the visual servoing task is given by the expected pixel s * and the current pixel s generates an error value e=ss * ,The reference controller is expressed as: In order to ensure that the optimized solution can ensure system stability, it is necessary to add CLF constraints to the CBF constraints obtained in step 4, considering the CLF of the preset time: where e = ss * , c1 is a coefficient greater than 0, s * For the desired pixel position, the first-order differentiation of CLF is obtained: Since e=ss * Medium * is a constant, then Through the second-order kinematics of IBVS and the second-order kinematics of the robot, we have: According to the definition of CLF and replacing the uncertain terms, a CLF form that satisfies the uncertainty is constructed. The constraint equations are: l1 and l2 are constants greater than 0 and 1 respectively. To ensure that the CLF converges within the specified time, m>2, b is a constant greater than zero, and M be is a greater than The constant of the upper bound; λ and μ are the terms that constitute the preset time CLF, which can ensure that the CLF is within the preset time T p convergence; The optimization objective function is designed as u n is the reference constraint without considering the safety issue, and u is the optimization result after considering the safety issue. In order to ensure the optimal energy, the optimization result u will not disappear or become very large; Adding a slack variable δ after the CLF constraint relaxes the convergence condition, then all safety constraint problems: CLF constraint, feature point visibility constraint, robot joint obstacle avoidance constraint, robot link obstacle avoidance constraint, robot joint angular velocity constraint, robot angular acceleration constraint are transformed into optimization problems:

Images