A dynamic visual servo control method with two-level disturbance compensation

Abstract

The present invention discloses a dynamic visual servo control method with two-stage disturbance compensation, which belongs to the technical field of visual servo control of mechanical arms. The method comprises the following steps: designing a posture and velocity estimator based on virtual visual servoing; using the end effector motion model and the posture and velocity estimator results, respectively designing a position Kalman filter and a rotation angle Kalman filter, filtering the estimation results and resynthesizing the posture matrix and velocity matrix; using a single-joint dynamics equation to independently design a nonlinear anti-disturbance controller for each joint of the mechanical arm; and introducing a sand cat swarm algorithm to perform parameter setting for the anti-disturbance controller. The present invention only uses image information to estimate the required information, and reduces the influence of image noise. The designed nonlinear anti-disturbance controller can estimate and compensate for uncertainty. The designed parameter setting method reduces the manual time consumed by parameter setting, and improves the control performance of visual servoing.

Description

Technical Field

[0001] The invention relates to the technical field of mechanical arm visual servo control, and in particular to a dynamic visual servo control method with two-stage disturbance compensation. Background Art

[0002] The robot visual servo system is a composite system with an image sensor, which is affected not only by the imaging system but also by the robotic arm system. During the imaging process, it is affected by calibration parameters such as resolution and lens distortion, and during data transmission, image noise and transmission delay, which reduce the accuracy of useful information extracted from the image. In addition, due to the high complexity of the robotic arm system, there are various uncertain factors that affect its control performance, such as modeling uncertainties such as unmodeled dynamics and external interference, which lead to the difficulty of controlling the system in terms of dynamic performance and stability.

[0003] Considering that the input signal of the visual controller is the image feature error, and the depth estimation of the image Jacobian matrix requires image information, the uncertainty is image noise, camera calibration error, visual delay, etc., which will affect the calculation of the image Jacobian matrix and ultimately affect the performance of the visual servo. When dealing with uncertainty, image-based visual servoing has higher robustness to camera calibration errors because it does not require three-dimensional reconstruction; for the remaining uncertainties, the lighting conditions can be improved in hardware or low-noise sensors can be used to ensure that as little image noise as possible is introduced during the image acquisition stage, or wavelet denoising, deep learning methods, filters, etc. can be used in software for processing. Kalman filtering, as a very classic filtering algorithm, can handle noisy dynamic systems and provide good system state prediction results, and has a good effect in reducing the impact of image noise. Using a pose and velocity estimator that combines virtual visual servoing and Kalman filtering in a visual servo system can reduce the impact of image noise and can efficiently estimate the required information.

[0004] If visual servo technology is only used in ordinary occasions such as sorting and spraying, kinematic visual servo can basically meet the task requirements, that is, assuming that the robot arm is an ideal Cartesian motion device that can accurately move to the required position according to the command. However, when the robot arm operates in a high-speed, dynamic or uncertain environment, it is necessary to consider the dynamic characteristics of the robot arm and study the technology based on dynamic visual servo. Traditional PID control has been difficult to meet the control requirements of dynamic visual servo systems and it is difficult to handle multiple uncertainties in the model. As a control method based on dynamic models, computational torque control can handle uncertainties related to factors such as gravity, speed and acceleration coupling. Under the condition that the robot arm can be accurately modeled and the parameters are accurate, its controller structure is simple and easy to use. However, since the parameters of the dynamic model change over time or its prior knowledge is not sufficient, it is difficult to establish an accurate dynamic model, making it difficult to obtain good results based on the model. ADRC does not require a detailed mathematical model to describe the input-output relationship. It can estimate and compensate for external interference, unmodeled dynamics and other uncertainties as total disturbances. Since its proposal, it has been applied in various fields of control engineering and has shown excellent results. Using an ADRC in a visual servo system can achieve high-precision dynamic servo control and handle various uncertainties.

[0005] In summary, for the robotic arm visual servo system with uncertainties such as image noise, unmodeled dynamics, and external disturbances, designing a dynamic visual servo control method with two-stage disturbance compensation has important theoretical and practical value. Summary of the invention

[0006] Aiming at the uncertainty problem existing in the visual servo system, the present invention provides a dynamic visual servo control method with two-stage disturbance compensation, which has reasonable design, solves the shortcomings of the prior art and has good effect.

[0007] In order to achieve the above object, the present invention adopts the following technical scheme:

[0008] A dynamic visual servo control method with two-stage disturbance compensation comprises the following steps:

[0009] Step 1: Design a pose and velocity estimator based on virtual visual servoing;

[0010] Step 2: Using the end effector motion model and the pose and velocity estimator results, design a position Kalman filter and a rotation angle Kalman filter respectively, filter the estimation results and then resynthesize the pose matrix and velocity matrix;

[0011] Step 3: Use the single-joint dynamics equation to independently design a nonlinear active disturbance rejection controller for each joint of the robotic arm;

[0012] Step 4: Use the empirical formula method to determine the value range for some parameters, and introduce the sand cat swarm algorithm to perform parameter tuning for the active disturbance rejection controller.

[0013] Furthermore, in step 1, the controlled object is a visual servo system of a serial robot arm, which is an eye-in-hand type, that is, a camera is installed at the end of the robot arm, and the camera is a real camera; a virtual camera is set, and the position and image acquired by the virtual camera are the same as those of the real camera. When the real camera reaches a new position, the current real image is acquired, and the virtual camera moves according to the image feature error between the virtual image and the real image. When the error converges, the relative position of the virtual camera and the virtual object is the relative position of the real camera and the real object; the specific process is as follows:

[0014] Let m represent the actual object point P j The corresponding image feature on the image plane, m * Represents the image feature corresponding to the virtual object point P0 on the image plane;

[0015] Define the space speed of the virtual object point c V vir and the time derivative of the virtual image feature The relationship is:

[0016]

[0017] In the formula, c P=[ c x, c y, c z] is the coordinate of the virtual object point P0 in the virtual camera coordinate system, Δt is the unit time interval, c v and c w is the linear velocity and angular velocity of the virtual object point, I is the unit matrix, 2d J 3d is the transformation matrix between two-dimensional points and three-dimensional points, is the three-dimensional point interaction matrix under the first-order approximation, L1 is the two-dimensional point interaction matrix, that is, the image Jacobian matrix;

[0018] m * The image feature error e between vir The definition is as follows:

[0019] e vir =m * -m; (2)

[0020] Let the image feature error e vir In the form of exponential convergence, we have:

[0021]

[0022] Where λ is the gain of the visual control law in virtual visual servoing;

[0023] Substituting equation (1) and equation (2) into equation (3), we obtain:

[0024]

[0025] In the formula, is the inverse matrix of the image Jacobian matrix in virtual visual servoing;

[0026] Any displacement of a rigid body in general motion can be decomposed into a translation increment δP i and the rotation increment δθ i , whose expression is:

[0027]

[0028]

[0029] In the formula, v(t) is the linear velocity of the rigid body, i represents the time from t0 to t i i time intervals at time t0, k represents the time interval from time t0 to time t k k time intervals at time instant, v k t k Time to t k+1 The linear velocity at a certain time, w(t) is the angular velocity of the rigid body, w k t k Time to t k+1 The angular velocity at time; the linear velocity v(t) and angular velocity w(t) are The amount of

[0030] δθ i is the rotation increment around a specific rotation axis, and its corresponding rotation matrix δR i This can be done by cross-producting the matrix [] × And the matrix exponential exp() is calculated to get:

[0031] δR i =exp([δθ i ] × ); (7)

[0032] During the duration iΔt=t i -t0, by using the speed command c V vir and acceleration command According to the displacement increment δP of rigid body motion i and δθ i To control the virtual camera to achieve the corresponding movement, the virtual image feature m *It will be updated to the new image feature m', and finally at t after repeated operations j At this moment, the image features converge to m, and e vir minimum, then the relative pose of the virtual camera and the virtual object point is the relative pose of the actual camera and the actual object point; when estimating the pose, the initial virtual camera and virtual image are given arbitrarily, and when estimating the speed, the estimation result of the previous moment is used as the initial setting of the virtual visual servo. After the pose estimation result is obtained according to the visual control law, The accumulated value of is the speed estimation result;

[0033] Under the premise of knowing the actual object point pose, the pose and velocity estimator is designed based on virtual visual servoing. The current image feature m and the initial virtual camera pose T0 are used as input to finally obtain the real camera pose matrix. and the velocity matrix Since the camera is installed in an eye-in-hand manner, the position and speed of the end effector of the robotic arm can be known.

[0034] Furthermore, in step 2, firstly, for the pose matrix and velocity matrix output by the pose and velocity estimator, the 4×4 pose matrix is ​​decomposed into position vectors t 3×1 With the rotation matrix R 3×3 , using formula (8) to get the rotation angle of the corresponding axis:

[0035]

[0036] Among them, θ x ,θ y ,θ z are the rotation angles around the x, y, and z axes, respectively, and R 11 is the element in the first row and first column of the rotation matrix; R 21 is the element in the second row and first column of the rotation matrix, R 31 is the element in the third row and first column of the rotation matrix, R 32 is the element in the third row and second column of the rotation matrix, R 33 is the element in the 3rd row and 3rd column of the rotation matrix;

[0037] Then, based on the knowledge of rigid body kinematics, the state space equations of the end effector position, rotation angle and velocity are obtained, namely, the motion model of the end effector;

[0038] The discrete time state equation and output equation of the position of the end effector of the six-degree-of-freedom manipulator are expressed as:

[0039]

[0040] Where X is the position and linear velocity of the end effector, U is the linear acceleration of the end effector, Y is the position and linear velocity estimation result provided by the estimator, ξ is the process noise vector, which is related to the system dynamic characteristics and environmental conditions, η is the measurement noise vector, which is related to image noise, camera calibration error, etc., and their covariances are Q and R respectively, that is, p(ξ)∈N(0,Q), p(η)∈N(0,R), A d , B d , C d is a constant matrix;

[0041] The position estimate in the pose matrix and the linear velocity estimate in the velocity matrix are used as measurement vectors, and the position Kalman filter is designed using the state space equation:

[0042]

[0043] In the formula, is the state prediction value, is the state estimate, K k is the Kalman gain, which represents the proportion of model prediction error and measurement error in the process of state optimal estimation, and ∑ is the state estimation value The covariance matrix of - is the state prediction value The covariance matrix of

[0044] The discrete time state equation and output equation of the rotation angle of the six-degree-of-freedom robot end effector are expressed as:

[0045]

[0046] Where X' is the rotation angle and angular velocity of the end effector, the input variable U' is the angular acceleration of the end effector, and the measurement vector Y' is the rotation angle and angular velocity estimation result provided by the estimator;

[0047] The estimated rotation angle in the pose matrix and the estimated angular velocity in the velocity matrix are used as measurement vectors to design a rotation angle Kalman filter:

[0048]

[0049] Finally, the rotation matrix is ​​obtained according to equations (13) and (14), and is combined with the filtered position vector to form a pose matrix;

[0050]

[0051] R=R z R y R x ; (14)

[0052] In the formula, R is the overall rotation matrix, R x , R y , R z are the rotation matrices corresponding to the rotation angles of the x, y, and z axes, respectively. i is the unit vector of the x axis, j is the unit vector of the y axis, and k is the unit vector of the z axis.

[0053] The filtered linear velocity vector and angular velocity vector are combined into a velocity matrix.

[0054] Furthermore, in step 3, the six-degree-of-freedom mechanical arm dynamics equation is simplified to a single-joint dynamics equation:

[0055]

[0056] Where q is the joint angle, is the joint angular velocity, is the joint angular acceleration, is the total disturbance, ω is the external disturbance, τ is the input torque, and b is the coefficient of the input torque;

[0057] Each driving joint of the six-degree-of-freedom manipulator is independently designed with a nonlinear active disturbance rejection controller NLADRC, which includes an extended state observer ESO, a nonlinear state error feedback control law NLSEF and a disturbance compensation device DCD.

[0058] ESO is used to estimate the state variables and total disturbance of the system Specifically:

[0059]

[0060] Where z1 is the tracking signal of the robot joint angle q, is the differential signal of z1, z2 is the first-order differential signal of the joint angle q, is the differential signal of z2, z3 is the estimated value of the total disturbance of the system, is the differential signal of z3, e is the tracking error of the joint angle; β 01 , β 01 , β 03 is the gain coefficient of ESO, α0 and α1 are user-defined nonlinear factors with values ​​between 0 and 1, δ0 and δ1 are user-defined filter factors, and b0 is the estimate of the compensation coefficient;

[0061] ESO obtains the observed values ​​of state variables z1, z2 and z3, and the visual servo system gives the joint angle command q d and joint speed commands NLSEF combines them in a nonlinear form and uses the fal function to construct a nonlinear control law, specifically:

[0062]

[0063] Where, e1 is the angle error, e2 is the angular velocity error, and q d and are the desired angle and desired velocity commands, τ0 is the original control torque of NLSEF, β1 and β2 are the gain coefficients of error and error differential, α3 is the user-defined nonlinear factor, δ2 and δ3 are the user-defined filter factors;

[0064] After ESO estimates the total disturbance of the system, DCD compensates for the disturbance as follows:

[0065] τ=τ0-z3 / b0; (19)

[0066] Where τ is the actual input torque of the robot, and z3 / b0 is the total disturbance compensation of the system.

[0067] Furthermore, the specific process in step 4 is as follows:

[0068] Step 4.1, initialize parameters; assume that the population size of sand cats is N, the dimension of the optimized parameters is n, the upper and lower limits of the search space are ub and lb respectively, and the maximum number of iterations is T; some parameters in the ADRC are set to α0 = 0.25, α1 = 0.5, δ0 = δ1 = 0.01, and the remaining parameters are adjusted using the sand cat swarm algorithm with a value range of β 01 ∈[0,150],β 02 ∈[0,7500],β 03 ∈[0,12500],β1∈[0,500],β2∈[0,50],b0∈[0,5];

[0069] Set the fitness function:

[0070]

[0071] Where J is the fitness value, w1, w2, w3, w4 are weight factors; e1 is the feedback error; e z1 、e z2 is the angle q and angular velocity observed by ESO The error of; u is the input torque;

[0072] Step 4.2, initialize the population; introduce the PWLCM chaotic mapping and initialize the PWLCM chaotic population, as shown below:

[0073]

[0074] P c =lb+(ub-lb)*x i+1 ; (twenty two)

[0075] In the formula, x i is the random number generated by the rand function, x i+1 is the random number after PWLCM chaotic mapping, F p is the overall function of the PWLCM chaotic mapping, p is the control parameter of the PWLCM chaotic mapping, p∈(0,0.5), x i ∈(0,1), i is the individual number of the current iteration; P c is the initial position of the individual sand cat, ub and lb are the upper and lower limits of the population position respectively;

[0076] Then, the fitness value of the sand cat individual is calculated according to the designed fitness function, and the best sand cat individual is selected as the best position P bc , the remaining individuals move toward this individual in subsequent iterations;

[0077] Step 4.3, search for the optimal parameter value, and perform search and attack behaviors;

[0078] Assume S M The auditory characteristics of the sand cat are simulated, with a value of 2 and an auditory sensitivity of r G The expression is:

[0079]

[0080] In the formula, t' is the current iteration number, T is the maximum iteration number;

[0081] In order to avoid falling into the local optimum, the sensitivity range of each sand cat is different, and its expression is:

[0082] r=r G *rand; (24)

[0083] In the formula, r represents the sensitivity range of each sand cat, and rand is a random number between 0 and 1;

[0084] Hearing sensitivity G As the number of iterations increases, it decreases linearly from 2 to 0, so a nonlinear adjustment strategy is introduced to describe the hunting process of the sand cat population:

[0085]

[0086] In the formula, η is the parameter that controls the nonlinearity of the curve;

[0087] Using formula (25) for r G Assignment, according to different η values, r G The curve also changes;

[0088] R is a parameter that controls the transition between search behavior and attack behavior, and its expression is:

[0089] R=2*r G *rand-r G ; (26)

[0090] When |R|>1, the cats start searching. Each cat searches for the best position P. bc 、Current location P c and sensitivity range r to update its own position, so the sand cat can find other possible best prey locations;

[0091] The mathematical model of the search behavior of sand cat colonies is:

[0092] P c (t+1)=r*(P bc (t)-rand*P c (t)); (27)

[0093] Where P c (t+1) is the location where the sand cat group moves to after performing the search behavior;

[0094] When |R|≤1, the sand cat will attack, first according to the best position P bc and the current position P c To generate a random position, the expression is:

[0095] P r =|rand*P bc (t)-P c (t)|; (28)

[0096] Assuming that the sensitivity range of the sand cat is a circle, the roulette method is used to randomly select an angle θ for each sand cat, and finally the attack behavior is realized through formula (29);

[0097] The mathematical model of the aggressive behavior of sand cat groups is:

[0098] P c '(t+1)=P c (t)-r*P r *cos(θ); (29)

[0099] Where P c '(t+1) is the position where the sand cat group moves to after the attack, P r is a random position;

[0100] Step 4.4, assign the parameter value represented by the sand cat individual in the current iteration to the NLADRC controller, and run the visual servo system respectively. According to different parameter values, the system will output different performance indicators and calculate their respective fitness values;

[0101] Step 4.5: Introduce an elimination mechanism to eliminate the sand cat individuals with the worst parameter setting effect, that is, the sand cat individuals with the highest fitness, and randomly generate new individuals according to the interval of the current sand cat group position in order to keep the total population unchanged;

[0102] Step 4.6: Determine whether the maximum number of iterations has been reached. If not, proceed to the next round of search and attack behavior to continue looking for the best prey location, that is, the optimal parameter value. If the maximum number of iterations has been reached, end the parameter tuning of the sand cat group and obtain the optimal NLADRC parameter value under the current number of iterations.

[0103] Beneficial technical effects brought by the present invention:

[0104] The present invention provides a dynamic visual servo control method with two-level disturbance compensation. A dynamic visual servo framework is designed using virtual visual servo, Kalman filter and anti-disturbance control technology to realize the control of the dynamic visual servo system of the robot arm. Its beneficial effects include: 1) a posture and velocity estimator based on virtual visual servo and Kalman filter is designed, which can estimate the required information only by using image information and reduce the influence of image noise. 2) Nonlinear anti-disturbance controllers are designed for each joint with different structural characteristics, which can estimate and compensate for uncertainty. 3) A parameter setting method based on the improved sand cat swarm optimization algorithm is designed, which makes the complex and tedious parameter setting work simple, reduces the manual time consumed by parameter setting, and improves the control performance of visual servo. BRIEF DESCRIPTION OF THE DRAWINGS

[0105] Figure 1 It is a framework diagram of dynamic visual servoing with two-level disturbance compensation in the present invention;

[0106] Figure 2 It is the principle diagram of virtual visual servoing in the present invention;

[0107] Figure 3 It is a structural block diagram of a posture and speed estimator based on virtual visual servoing in the present invention;

[0108] Figure 4 This is a schematic diagram of the image disturbance suppression method based on the Kalman filter in the present invention;

[0109] Figure 5 This is a schematic diagram of the self-disturbance rejection control principle of a single joint system in the present invention;

[0110] Figure 6It is a parameter setting flow chart based on the improved sand cat swarm algorithm in the present invention; DETAILED DESCRIPTION

[0111] The specific implementation of the present invention is further described below in conjunction with the accompanying drawings and specific embodiments:

[0112] like Figure 1 As shown in the traditional dynamic visual servoing framework, the expected image feature m d The image feature error e between the actual image feature m f The speed command V is obtained through the visual controller and the image Jacobian matrix. d , and then converted into joint velocity instructions through space transformation and robot Jacobian matrix After passing through the robot controller, the control torque τ is input to the robot. In the visual control part, the present invention uses virtual visual servo technology to design a posture and velocity estimator, first obtains the relative posture of the actual camera and the actual object point, extracts the depth information from the relative posture matrix, and provides the depth information to the image Jacobian matrix; then estimates the posture and velocity matrix of the actual camera based on the actual object point information, and after both are processed by Kalman filtering, the posture matrix is ​​converted into angle information through inverse kinematics, and the angle information and velocity matrix are provided to the NLADRC controller;

[0113] In addition, based on the results of the motion model and estimator of the end effector, the influence of image noise is reduced by designing a Kalman filter. In the robotic arm joint control part, the present invention uses an anti-disturbance control that does not rely on an accurate model to achieve dynamics-based visual servoing, and uses an extended state observer to estimate and compensate for disturbances such as unmodeled dynamics and external disturbances. The present invention processes image noise and modeling uncertainty in the visual and robotic arm parts respectively, and the virtual visual servoing, Kalman filtering and anti-disturbance control technologies used are all simple to implement, and are a feasible solution.

[0114] A dynamic visual servo control method with two-stage disturbance compensation comprises the following steps:

[0115] Step 1: Design a pose and velocity estimator based on virtual visual servoing;

[0116] The controlled object is a visual servo system of a serial robot arm. The system is an eye-in-hand type, that is, a camera is installed at the end of the robot arm, and the camera is a real camera. A virtual camera is set. The position and image acquired by the virtual camera are the same as those of the real camera. When the real camera reaches a new position, the current real image is acquired. The virtual camera moves according to the image feature error between the virtual image and the real image. When the error converges, the relative position of the virtual camera and the virtual object is the relative position of the real camera and the real object.

[0117] like Figure 2 As shown, m represents the actual object point P j The corresponding image feature on the image plane, m * Represents the image features corresponding to the virtual object point P0 on the image plane; the position of the virtual object point, the position of the virtual camera, and the relative position and posture of the two c T o Under the premise of knowing, the virtual object point or virtual camera is moved according to a certain motion law, and the virtual image point is continuously updated. * When converging to the actual image feature m, the relative pose of the virtual camera and the virtual object point c T o δT j That is, the relative pose of the actual camera and the actual object point. At this time, if the pose information of one of them is known, the pose information of the other can be obtained.

[0118] The above-mentioned certain motion rules refer to the concept related to visual servoing, that is, to calculate the speed command and control the movement of the robot arm based on the image feature error. Define the spatial speed of the virtual object point c V vir and the time derivative of the virtual image feature The relationship is:

[0119]

[0120] In the formula, c P=[ c x, c y, c z] is the coordinate of the virtual object point P0 in the virtual camera coordinate system, Δt is the unit time interval, c v and c w is the linear velocity and angular velocity of the virtual object point, I is the unit matrix, 2d J 3d is the transformation matrix between two-dimensional points and three-dimensional points, L Pi is the three-dimensional point interaction matrix under the first-order approximation, L1 is the two-dimensional point interaction matrix, that is, the image Jacobian matrix;

[0121] m * The image feature error e between vir The definition is as follows:

[0122] e vir =m * -m; (2)

[0123] Let the image feature error e vir In the form of exponential convergence, we have:

[0124]

[0125] Where λ is the gain of the visual control law in virtual visual servoing;

[0126] Substituting equation (1) and equation (2) into equation (3), we obtain:

[0127]

[0128] In the formula, is the inverse matrix of the image Jacobian matrix in virtual visual servoing;

[0129] Any displacement of a rigid body in general motion can be decomposed into a translation increment δP i and the rotation increment δθ i , whose expression is:

[0130]

[0131]

[0132] In the formula, v(t) is the linear velocity of the rigid body, i represents the time from t0 to t i i time intervals at time t0, k represents the time interval from time t0 to time t k k time intervals at time instant, v k t k Time to t k+1 The linear velocity at a certain time, w(t) is the angular velocity of the rigid body, w k t k Time to t k+1 The angular velocity, linear velocity v(t) and angular velocity w(t) at time instant are The amount of

[0133] δθ i is the rotation increment around a specific rotation axis, and its corresponding rotation matrix δR i This can be done by cross-producting the matrix [] × And the matrix exponential exp() is calculated to get:

[0134] δR i =exp([δθ i ] × ); (7)

[0135] During the duration iΔt=t i -t0, by using the speed command c V vir and acceleration command According to the displacement increment δP of rigid body motion i and δθ i To control the virtual camera to achieve the corresponding movement, the virtual image feature m* It will be updated to the new image feature m', and finally at t after repeated operations j At this moment, the image features converge to m, and e vir When estimating the pose, the initial virtual camera and virtual image are given arbitrarily. When estimating the speed, the estimation result of the previous moment is used as the initial setting of the virtual visual servo. After the pose estimation result is obtained according to the visual control law, the speed The accumulated value of is the speed estimation result;

[0136] If the three-dimensional information of the actual object point is known, the actual camera pose can be obtained through coordinate transformation. If the three-dimensional information is not clearly given, virtual visual servoing can be used to estimate the three-dimensional information of the actual object point based on the camera and virtual object point with known fixed pose. The principle is the same as estimating the camera pose, the only difference is that the known conditions are different.

[0137] Figure 3 The block diagram of the pose and velocity estimator based on virtual visual servoing is shown in Figure 2. The pose and velocity estimator based on virtual visual servoing takes the current image feature m and the initial virtual camera pose T0 as input, and finally obtains the pose and velocity estimation results, which are Since the camera is installed in an eye-in-hand manner, the position and speed of the end effector of the robot arm can be known. In one operation cycle, the speed and acceleration instructions are calculated based on the characteristic error between the current image and the virtual image, and then the virtual camera movement is controlled according to the general formula of rigid body motion. The corresponding virtual image can be generated according to the current virtual camera's position and camera model. After multiple cycles, a virtual image that coincides with the actual image is obtained, that is, the image error meets the accuracy requirements. At this time, the accumulated value of the speed and position is the estimated result of the current speed and position.

[0138] It should be noted that when estimating the pose, the initial virtual camera extrinsic parameters and virtual image are given arbitrarily. However, when estimating the speed, the estimation result at the previous moment is required as the initial setting of the virtual visual servoing. After obtaining the pose estimation result according to the visual servoing control law, the accumulated value of the speed is the speed estimation result.

[0139] Step 2: Using the end effector motion model and the pose and velocity estimator results, design a position Kalman filter and a rotation angle Kalman filter respectively, filter the estimation results and then resynthesize the pose matrix and velocity matrix;

[0140] Although a separate pose and velocity estimator can estimate pose and velocity, its estimation accuracy depends on the image measurement accuracy and noise level. In order to improve the estimation accuracy and reduce the influence of noise, the pose and velocity estimator is regarded as a measurement device, and the position and rotation angle Kalman filter is designed using the end effector motion model.

[0141] Figure 4 The schematic diagram of the image disturbance suppression method based on the Kalman filter is shown in Figure 1, where the Kalman filter can filter the estimation result to reduce the influence of inevitable image noise. The specific process is:

[0142] First, the 4×4 pose matrix in the output of the pose and velocity estimator is decomposed into the position vector t 3×1 With the rotation matrix R 3×3 , using formula (8) to get the rotation angle of the corresponding axis:

[0143]

[0144] Among them, R 11 is the element in the first row and first column of the rotation matrix; R 21 is the element in the second row and first column of the rotation matrix, R 31 is the element in the third row and first column of the rotation matrix, R 32 is the element in the third row and second column of the rotation matrix, R 33 is the element in the 3rd row and 3rd column of the rotation matrix;

[0145] Then, based on the knowledge of rigid body kinematics, the state space equations of the end effector position, rotation angle and velocity are obtained, namely, the motion model of the end effector;

[0146] The discrete time state equation and output equation of the position of the end effector of the six-degree-of-freedom manipulator are expressed as:

[0147]

[0148] Where X is the position and linear velocity of the end effector, U is the linear acceleration of the end effector, Y is the position and linear velocity estimation result provided by the estimator, ξ is the process noise vector, which is related to the system dynamic characteristics and environmental conditions, η is the measurement noise vector, which is related to image noise, camera calibration error, etc., and their covariances are Q and R respectively, that is, p(ξ)∈N(0,Q), p(η)∈N(0,R), A d , B d , C d is a constant matrix;

[0149] Secondly, the position estimate in the pose matrix and the linear velocity estimate in the velocity matrix are used as measurement vectors, and the position Kalman filter is designed using the state space equation as follows:

[0150]

[0151] In the formula, is the state prediction value, is the state estimate, K k is the Kalman gain, which represents the proportion of model prediction error and measurement error in the process of state optimal estimation, and ∑ is the state estimation value The covariance matrix of - is the state prediction value The covariance matrix of the position Kalman filter is: the first two formulas are called the time update process, that is, the current state is predicted based on the position, velocity and acceleration of the previous moment, and the last three formulas are called the state update process, that is, the best estimated state is obtained through the predicted value and the observed value. The state estimation result given by the position Kalman filter is the position and linear velocity estimation result after filtering, which reduces the influence of noise to a certain extent and can obtain satisfactory results.

[0152] The discrete time state equation and output equation of the rotation angle of the end effector of the six-degree-of-freedom manipulator can be expressed as:

[0153]

[0154] Where X' is the rotation angle and angular velocity of the end effector, the input variable U' is the angular acceleration of the end effector, and the measurement vector Y' is the rotation angle and angular velocity estimation result provided by the estimator. After obtaining the estimated results of the position and rotation angle, the kinematics knowledge can be used to convert them into angle estimation results and provide them to the robot control loop.

[0155] The estimated rotation angle in the pose matrix and the estimated angular velocity in the velocity matrix are used as measurement vectors to design a rotation angle Kalman filter:

[0156]

[0157] Finally, the rotation matrix is ​​obtained according to equations (13) and (14), and is combined with the filtered position vector to form a pose matrix;

[0158]

[0159] R=R z R y R x ; (14)

[0160] In the formula, R is the overall rotation matrix, R x, R y , R z They are the rotation matrices corresponding to the rotation angles of the x, y, and z axes, respectively. i is the unit vector of the x axis, j is the unit vector of the y axis, and k is the unit vector of the z axis.

[0161] The filtered linear velocity vector and angular velocity vector are combined into a velocity matrix.

[0162] Step 3: Use the single-joint dynamics equation to independently design a nonlinear active disturbance rejection controller for each joint of the robotic arm;

[0163] The interaction and coupling effects between joints during the motion process can be considered as disturbances acting on a single joint system, so each joint of the robot is regarded as a single-input single-output system for control. The dynamic equation of the six-degree-of-freedom robot is simplified to the single-joint dynamic equation:

[0164]

[0165] Where q is the joint angle, is the joint angular velocity, is the joint angular acceleration, is the total disturbance, which is related to the coupling effect and uncertainty of the system; ω is the external disturbance, τ is the input torque, and b is the coefficient of the input torque;

[0166] Due to the different structural characteristics of each joint of the robot arm, the present invention will design a nonlinear active disturbance rejection controller (NLADRC) controller for each driving joint of the six-degree-of-freedom robot arm. The principle diagram of the active disturbance rejection control of a single joint system is shown in Figure 5 As shown. NLADRC usually consists of four components: the tracking differentiator (TD) can quickly track the input signal and generate an approximate differential signal; the extended state observer (ESO) regards external disturbances and unmodeled dynamics as total disturbances, and estimates them as the system's extended state in real time; the nonlinear state error feedback control law (NLSEF) combines the tracking output signal and the state feedback signal in a nonlinear form to provide a control quantity; the disturbance compensation device (DCD) performs reasonable compensation based on the estimated total disturbance, and can approximate the original system as an integral series system to control and correct the unprocessed input. The present invention can achieve the given tracking signal and differential signal of the robot controller in each operating cycle by setting the expected image features, thereby canceling TD and using the visual control loop to directly give the input signal of the robot control loop. ESO is the core of the self-disturbance rejection control, which is used to estimate the state variables and total disturbance of the system. Therefore, the operating characteristics of ESO have a significant impact on the control performance of the active disturbance rejection control. ESO does not rely on the disturbance model, nor does it need to obtain the disturbance estimate through direct measurement. The specific algorithm is as follows:

[0167]

[0168]

[0169] Where z1 is the tracking signal of the robot joint angle q, z2 is the first-order differential signal of the joint angle q, z3 is the estimated value of the total disturbance of the system, and e is the tracking error of the joint angle; β 01 , β 01 , β 03 is the gain coefficient of ESO, α0 and α1 are user-defined nonlinear factors with values ​​between 0 and 1, δ0 and δ1 are user-defined filter factors, and b0 is the estimate of the compensation coefficient

[0170] ESO obtains the observed values ​​of state variables z1, z2 and z3, and the visual servo system gives the angle q d and speed command NLSEF combines them in a nonlinear form and constructs a nonlinear control law using the fal function. It has the advantages of fast convergence, good robustness and adaptability, small error, large gain; large error, small gain, specifically:

[0171]

[0172] Where, e1 is the angle error, e2 is the angular velocity error, and q d and are the desired angle and desired velocity commands, τ0 is the original control torque of NLSEF, β1 and β2 are the gain coefficients of error and error differential, α3 is the user-defined nonlinear factor, δ2 and δ3 are the user-defined filter factors;

[0173] Disturbance compensation does not distinguish between internal and external disturbances. All coupling effects, unmodeled dynamics, etc. are regarded as the total disturbance of the system. After ESO estimates the total disturbance of the system, DCD compensates for the disturbance as follows:

[0174] τ=τ0-z3 / b0; (19)

[0175] Where τ is the actual input torque of the robot, and z3 / b0 is the total disturbance compensation of the system. As the core controller of the entire control system, NLADRC can estimate the total disturbance in time and control the input torque by compensating the disturbance, making the visual servo system more robust and responsive.

[0176] Step 4: Use the empirical formula method to determine the value range for some parameters, and introduce the sand cat swarm algorithm to perform parameter tuning for the active disturbance rejection controller.

[0177] As a control method that does not rely on an accurate model, ADRC can estimate and compensate for total disturbances, but it has many parameters and is difficult to adjust, which affects the application of ADRC in engineering. The bandwidth method simplifies the difficulty of adjusting the parameters of the linear ADRC, that is, through the empirical formula β 01 =3ω,β 02 =3ω 2 , β 03 =ω 3 Manual parameter adjustment. The bandwidth-based empirical formula method is simple and effective for linear ADRC parameter adjustment, but it is not fully competent for parameter adjustment of nonlinear ADRC with more parameters. For this reason, the present invention sets the initial values ​​of some parameters based on the empirical formula method and introduces the sand cat group optimization algorithm for parameter adjustment.

[0178] The Sand Cat Swarm Optimization (SCSO) algorithm is a meta-heuristic algorithm designed based on the behavior of sand cats in nature. The Sand Cat Swarm Optimization algorithm mainly simulates two behaviors of sand cats: search behavior and attack behavior. Since sand cats live independently in nature, in order to propose the concept of swarm intelligence, it is assumed that sand cats are swarm-like. The parameter setting flow chart based on the improved Sand Cat Swarm Algorithm is shown in the figure below: Figure 6 The specific description is as follows:

[0179] Step 4.1, initialization parameters; Assume that the population size of sand cats is N, the dimension of the optimized parameters is n, the upper and lower limits of the search space are ub and lb respectively, and the maximum number of iterations is T. Some parameters in the ADRC are set to α0 = 0.25, α1 = 0.5, δ0 = δ1 = 0.01, and the remaining parameters are adjusted using the sand cat swarm algorithm, and their value range is β 01 ∈[0,150],β 02 ∈[0,7500],β 03 ∈[0,12500],β1∈[0,500],β2∈[0,50],b0∈[0,5];

[0180] Set the fitness function:

[0181] J=∫((w1|e1|+w2|e z1 |+w3|e z2 |+w4|u|)*t)dt; (20)

[0182] Wherein, J is the fitness value, w1, w2, w3, and w4 are weight factors. In this embodiment, the weight factors are 0.55, 0.3, 0.3, and 0.01 respectively; e1 is the feedback error; e z1 、e z2 is the angle q and angular velocity observed by ESO The error of; u is the input torque;

[0183] Step 4.2, initialize the population; the distribution of the initial population in the sand cat swarm algorithm will affect the convergence speed of the algorithm. It is difficult to generate a uniformly distributed population in space through a pseudo-random function. In order to increase the diversity of the population and make its distribution more uniform, the PWLCM chaotic mapping can be introduced to initialize the PWLCM chaotic population, as shown below:

[0184]

[0185] P c =lb+(ub-lb)*x i+1 ; (twenty two)

[0186] In the formula, x i is the random number generated by the rand function, x i+1 is the random number after PWLCM chaotic mapping, F p is the overall function of the PWLCM chaotic mapping, p is the control parameter of the PWLCM chaotic mapping, p∈(0,0.5), x i ∈(0,1), i is the individual number of the current iteration; P c is the initial position of the individual sand cat, ub and lb are the upper and lower limits of the population position respectively;

[0187] Then, the fitness value of the sand cat individual is calculated according to the designed fitness function, and the best sand cat individual is selected as the best position P bc , the remaining individuals move toward this individual in subsequent iterations;

[0188] Step 4.3, search for the optimal parameter value, and perform search and attack behaviors;

[0189] The ability of the sand cat to search for prey relies on the emission of low-frequency noise, and its auditory sensitivity is assumed to be in the range of 0-2kHz. M The auditory characteristics of the sand cat are simulated, with a value of 2 and an auditory sensitivity of r G The expression is:

[0190]

[0191] In the formula, t' is the current iteration number, T is the maximum iteration number;

[0192] In order to avoid falling into the local optimum, the sensitivity range of each sand cat is different, and its expression is:

[0193] r=r G *rand; (24)

[0194] In the formula, r represents the sensitivity range of each sand cat, and rand is a random number between 0 and 1;

[0195] Hearing sensitivity G As the number of iterations increases, it decreases linearly from 2 to 0, which is inconsistent with the natural law that the sand cat population needs multiple rounds of collaboration to hunt prey, and will also cause a linear transformation of the fluctuation range. Therefore, a nonlinear adjustment strategy is introduced to describe the hunting process of the sand cat population:

[0196]

[0197] In the formula, η is the parameter that controls the nonlinearity of the curve;

[0198] Using formula (25) for r G Assignment, according to different η values, r G The curve also changes, and the value is usually taken according to the actual situation;

[0199] R is a parameter that controls the transition between search behavior and attack behavior, and its expression is:

[0200] R=2*r G *rand-r G ; (26)

[0201] When |R|>1, the cats start searching. Each cat searches for the best position P. bc 、Current location P c And sensitivity range r G Updates its own position, so the sand cat can find other possible best prey locations;

[0202] The mathematical model of the search behavior of sand cat colonies is:

[0203] P c (t+1)=r*(P bc (t)-rand*P c (t)); (27)

[0204] Where P c (t+1) is the location where the sand cat group moves to after performing the search behavior;

[0205] When |R|≤1, the sand cat will attack, first according to the best position P bc and the current position P c To generate a random position, the expression is:

[0206] P r =|rand*P bc (t)-P c (t)|; (28)

[0207] Assuming that the sensitivity range of the sand cat is a circle, the roulette method is used to randomly select an angle θ for each sand cat, and finally the attack behavior is realized through formula (29); among them, the random position can ensure that the sand cat is close to the prey, and the random angle can prevent the algorithm from falling into the local optimal solution.

[0208] The mathematical model of the aggressive behavior of sand cat groups is:

[0209] P c '(t+1)=P c (t)-r*P r *cos(θ); (29)

[0210] Where P c '(t+1) is the position where the sand cat group moves to after the attack, P r is a random position;

[0211] Step 4.4, assign the parameter value represented by the sand cat individual in the current iteration to the NLADRC controller, and run the visual servo system respectively. According to different parameter values, the system will output different performance indicators and calculate their respective fitness values;

[0212] Step 4.5, introduce the elimination mechanism. In order to improve the search ability of the sand cat group algorithm, learn from the natural law of survival of the fittest in nature, and eliminate the sand cat individuals with the worst parameter setting effect in each iteration, that is, the sand cat individuals with the highest fitness. In order to keep the total population unchanged, new individuals are randomly generated according to the interval of the current sand cat group position;

[0213] Step 4.6: Determine whether the maximum number of iterations has been reached. If not, proceed to the next round of search and attack behavior to continue looking for the best prey location, that is, the optimal parameter value. If the maximum number of iterations has been reached, end the parameter tuning of the sand cat group and obtain the optimal NLADRC parameter value under the current number of iterations.

[0214] The above is the complete implementation process of this embodiment.

[0215] Of course, the above description is not a limitation of the present invention, and the present invention is not limited to the above examples. Changes, modifications, additions or substitutions made by technicians in this technical field within the essential scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A dynamic visual servo control method with two-stage disturbance compensation, characterized in that: The following steps are involved: Step 1: Design a pose and velocity estimator based on virtual visual servoing; Step 2: Using the end effector motion model and the pose and velocity estimator results, design a position Kalman filter and a rotation angle Kalman filter respectively, filter the estimation results and then resynthesize the pose matrix and velocity matrix; Step 3: Use the single-joint dynamics equation to independently design a nonlinear active disturbance rejection controller for each joint of the robotic arm; Step 4: Use the empirical formula method to determine the value range for some parameters, and introduce the sand cat group algorithm to perform parameter tuning for the active disturbance rejection controller; The specific process in step 4 is as follows: Step 4.1, initialize parameters; assume that the population size of sand cats is N, the dimension of the optimized parameters is n, the upper and lower limits of the search space are ub and lb respectively, and the maximum number of iterations is T; some parameters in the ADRC are set to α0 = 0.25, α1 = 0.5, δ0 = δ1 = 0.01, and the remaining parameters are adjusted using the sand cat swarm algorithm with a value range of β 01 ∈[0,150],β 02 ∈[0,7500],β 03 ∈[0,12500],β1∈[0,500],β2∈[0,50],b0∈[0,5]; Set the fitness function: J=∫((w1|e1|+w2|e z1 |+w3|e z2 |+w4|u|)*t)dt; (20) Where J is the fitness value, w1, w2, w3, w4 are weight factors; e1 is the feedback error; e z1 、e z2 is the angle q and angular velocity observed by ESO The error of; u is the input torque; Step 4.2, initialize the population; introduce the PWLCM chaotic mapping and initialize the PWLCM chaotic population, as shown below: P c =lb+(ub-lb)*x i+1 ; (22) In the formula, x i is the random number generated by the rand function, x i+1 is the random number after PWLCM chaotic mapping, F p is the overall function of the PWLCM chaotic mapping, p is the control parameter of the PWLCM chaotic mapping, p∈(0,0.5), x i ∈(0,1), i is the individual number of the current iteration; P c is the initial position of the individual sand cat; Then, the fitness value of the sand cat individual is calculated according to the designed fitness function, and the best sand cat individual is selected as the best position P bc , the remaining individuals move toward this individual in subsequent iterations; Step 4.3, search for the optimal parameter value, and perform search and attack behaviors; Assume S M The auditory characteristics of the sand cat are simulated, with a value of 2 and an auditory sensitivity of r G The expression is: In the formula, t' is the current iteration number, T is the maximum iteration number; To avoid falling into the local optimum, the sensitivity range of each sand cat is different, and its expression is: r=r G *rand; (24) In the formula, r represents the sensitivity range of each sand cat, and rand is a random number between 0 and 1; Hearing sensitivity G As the number of iterations increases, it decreases linearly from 2 to 0, so a nonlinear adjustment strategy is introduced to describe the hunting process of the sand cat population: In the formula, η is the parameter that controls the nonlinearity of the curve; Using formula (25) for r G Assignment, according to different η values, r G The curve also changes; W is a parameter that controls the transition between search behavior and attack behavior, and its expression is: W=2*r G *rand-r G ; (26) When |W|>1, the cats start searching. Each cat searches for the best position P. bc 、Current location P c and sensitivity range r to update its own position, so the sand cat can find other possible best prey locations; The mathematical model of the search behavior of sand cat colonies is: P c (t+1)=r*(P bc (t)-rand*P c (t)); (27) Where P c (t+1) is the location where the sand cat group moves to after performing the search behavior; When |W|≤1, the sand cat will attack, first according to the best position P bc and the current position P c To generate a random position, the expression is: P r =|rand*P bc (t)-P c (t)|; (28) Assuming that the sensitivity range of the sand cat is a circle, the roulette method is used to randomly select an angle θ for each sand cat, and finally the attack behavior is realized through formula (29); The mathematical model of the aggressive behavior of sand cat groups is: P c '(t+1)=P c (t)-r*P r *cos(θ); (29) Where P c '(t+1) is the position where the sand cat group moves to after the attack, P r is a random position; Step 4.4, assign the parameter value represented by the sand cat individual in the current iteration to the NLADRC controller, and run the visual servo system respectively. According to different parameter values, the system will output different performance indicators and calculate their respective fitness values; Step 4.5: Introduce an elimination mechanism to eliminate the sand cat individuals with the worst parameter setting effect, that is, the sand cat individuals with the highest fitness, and randomly generate new individuals according to the interval of the current sand cat group position in order to keep the total population unchanged; Step 4.6: Determine whether the maximum number of iterations has been reached. If not, proceed to the next round of search and attack behaviors to continue looking for the best prey location, that is, the best parameter value; If the maximum number of iterations is reached, the parameter tuning of the sand cat group is terminated, and the optimal NLADRC parameter value under the current number of iterations is obtained.

2. A dynamic visual servo control method with two-stage disturbance compensation according to claim 1, characterized in that: In the step 1, the controlled object is a visual servo system of a serial robot arm, which is an eye-in-hand type, that is, a camera is installed at the end of the robot arm, and the camera is a real camera; a virtual camera is set, and the position and image acquired by the virtual camera are the same as those of the real camera. When the real camera reaches a new position, the current real image is acquired, and the virtual camera moves according to the image feature error between the virtual image and the real image. When the error converges, the relative position of the virtual camera and the virtual object is the relative position of the real camera and the real object; the specific process is as follows: Let m represent the actual object point P j The corresponding image feature on the image plane, m * Represents the image feature corresponding to the virtual object point P0 on the image plane; Define the space speed of the virtual object point c V vir and the time derivative of the virtual image feature The relationship is: In the formula, c P=[ c x, c y, c z] is the coordinate of the virtual object point P0 in the virtual camera coordinate system, Δt is the unit time interval, c w is the angular velocity of the virtual object point, I is the unit matrix, 2d J 3d is the transformation matrix between two-dimensional points and three-dimensional points, is the three-dimensional point interaction matrix under the first-order approximation, L1 is the two-dimensional point interaction matrix, that is, the image Jacobian matrix; m * The image feature error e between vir The definition is as follows: e vir =m * -m; (2) Let the image feature error e vir In the form of exponential convergence, we have: Where λ is the gain of the visual control law in virtual visual servoing; Substituting equation (1) and equation (2) into equation (3), we obtain: In the formula, is the inverse matrix of the image Jacobian matrix in virtual visual servoing; Any displacement of a rigid body in general motion can be decomposed into a translation increment δP i and the rotation increment δθ i , whose expression is: In the formula, v(t) is the linear velocity of the rigid body, i represents the time from t0 to t i i time intervals at time t0, k represents the time interval from time t0 to time t k k time intervals at time, v k t k Time to t k+1 The linear velocity at a certain time, w(t) is the angular velocity of the rigid body, w k t k Time to t k+1 The angular velocity at time; the linear velocity v(t) and angular velocity w(t) are The amount of δθ i is the rotation increment around a specific rotation axis, and its corresponding rotation matrix δR i By cross-producting the matrix [ ] × And the matrix exponential exp() is calculated to get: δR i =exp([δθ i ] × ); (7) During the duration iΔt=t i -t0, by using the speed command c V vir and acceleration command According to the displacement increment δP of rigid body motion i and δθ i To control the virtual camera to achieve the corresponding movement, the virtual image feature m * It will be updated to the new image feature m', and finally at t after repeated operations j At this moment, the image features converge to m, and e vir minimum, then the relative pose of the virtual camera and the virtual object point is the relative pose of the actual camera and the actual object point; when estimating the pose, the initial virtual camera and virtual image are given arbitrarily, and when estimating the speed, the estimation result of the previous moment is used as the initial setting of the virtual visual servo. After the pose estimation result is obtained according to the visual control law, The accumulated value of is the speed estimation result; Under the premise of knowing the actual object point pose, the pose and velocity estimator is designed based on virtual visual servoing. The current image feature m and the initial virtual camera pose T0 are used as input to finally obtain the real camera pose matrix. and the velocity matrix Since the camera is installed in an eye-in-hand manner, the position and speed of the end effector of the robotic arm can be known.

3. A dynamic visual servo control method with two-stage disturbance compensation according to claim 2, characterized in that: In step 2, first, for the pose matrix and velocity matrix output by the pose and velocity estimator, the 4×4 pose matrix is ​​decomposed into the position vector t 3×1 With the rotation matrix R 3×3 , using formula (8) to get the rotation angle of the corresponding axis: Among them, θ x ,θ y ,θ z are the rotation angles around the x, y, and z axes, respectively, and R 11 is the element in the first row and first column of the rotation matrix; R 21 is the element in the second row and first column of the rotation matrix, R 31 is the element in the third row and first column of the rotation matrix, R 32 is the element in the third row and second column of the rotation matrix, R 33 is the element in the 3rd row and 3rd column of the rotation matrix; Then, based on the knowledge of rigid body kinematics, the state space equations of the end effector position, rotation angle and velocity are obtained, namely, the motion model of the end effector; The discrete time state equation and output equation of the position of the end effector of the six-degree-of-freedom manipulator are expressed as: Where X is the position and linear velocity of the end effector, U is the linear acceleration of the end effector, Y is the position and linear velocity estimation results provided by the estimator, ξ is the process noise vector, which is related to the system dynamic characteristics and environmental conditions, η is the measurement noise vector, which is related to the image noise and camera calibration error, and their covariances are Q and R respectively, that is, p(ξ)∈N(0,Q), p(η)∈N(0,R), A d , B d , C d is a constant matrix; The position estimate in the pose matrix and the linear velocity estimate in the velocity matrix are used as measurement vectors, and the position Kalman filter is designed using the state space equation: In the formula, is the state prediction value, is the state estimate, K k is the Kalman gain, which represents the proportion of model prediction error and measurement error in the process of state optimal estimation, and ∑ is the state estimation value The covariance matrix of - is the state prediction value The covariance matrix of The discrete time state equation and output equation of the rotation angle of the six-degree-of-freedom robot end effector are expressed as: Where X' is the rotation angle and angular velocity of the end effector, the input variable U' is the angular acceleration of the end effector, and the measurement vector Y' is the rotation angle and angular velocity estimation result provided by the estimator; The estimated rotation angle in the pose matrix and the estimated angular velocity in the velocity matrix are used as measurement vectors to design a rotation angle Kalman filter: Finally, the rotation matrix is ​​obtained according to equations (13) and (14), and is combined with the filtered position vector to form a pose matrix; R=R z R y R x ; (14) In the formula, R is the overall rotation matrix, R x , R y , R z are the rotation matrices corresponding to the rotation angles of the x, y, and z axes, respectively. i is the unit vector of the x axis, j is the unit vector of the y axis, and k is the unit vector of the z axis. The filtered linear velocity vector and angular velocity vector are combined into a velocity matrix.

4. A dynamic visual servo control method with two-stage disturbance compensation according to claim 3, characterized in that: In step 3, the dynamic equation of the six-degree-of-freedom manipulator is simplified to the single-joint dynamic equation: Where q is the joint angle, is the joint angular velocity, is the joint angular acceleration, is the total disturbance, ω is the external disturbance, τ is the input torque, and b is the coefficient of the input torque; Each driving joint of the six-degree-of-freedom manipulator is independently designed with a nonlinear active disturbance rejection controller NLADRC, which includes an extended state observer ESO, a nonlinear state error feedback control law NLSEF and a disturbance compensation device DCD. ESO is used to estimate the state variables and total disturbance of the system Specifically: Where z1 is the tracking signal of the robot joint angle q, is the differential signal of z1, z2 is the first-order differential signal of the joint angle q, is the differential signal of z2, z3 is the estimated value of the total disturbance of the system, is the differential signal of z3, e is the tracking error of the joint angle; β 01 , β 01 , β 03 is the gain coefficient of ESO, α0 and α1 are user-defined nonlinear factors with values ​​between 0 and 1, δ0 and δ1 are user-defined filter factors, and b0 is the estimate of the compensation coefficient; ESO obtains the observed values ​​of state variables z1, z2 and z3, and the visual servo system gives the joint angle command q d and joint speed commands NLSEF combines them in a nonlinear form and uses the fal function to construct a nonlinear control law, specifically: Where, e1 is the angle error, e2 is the angular velocity error, and q d and are the desired angle and desired velocity commands, τ0 is the original control torque of NLSEF, β1 and β2 are the gain coefficients of error and error differential, α3 is the user-defined nonlinear factor, δ2 and δ3 are the user-defined filter factors; After ESO estimates the total disturbance of the system, DCD compensates for the disturbance as follows: τ = τ0 - z3 / b0; (19) Where τ is the actual input torque of the robot, and z3 / b0 is the total disturbance compensation of the system.

Images