A visual servo control method based on event-triggered model prediction

Abstract

The present invention discloses a visual servo control method based on event-triggered model prediction, comprising the following steps: constructing a visual servo system structure, constructing a depth-independent model as a controlled object based on image information and joint information of a mechanical arm; designing a model prediction controller using the depth-independent model, and obtaining a predicted optimal input and optimal state sequence under specified constraints; designing an event triggering mechanism through deviation information before the optimal state generated by the image information and the model prediction controller; if the event triggering condition is met, the model prediction controller issues a control instruction to the mechanical arm; if the servo task is not completed, the feature point matching information and joint information are used to estimate the system parameters and the three-dimensional position information of the feature points in real time, and update the depth-independent model. The present invention can complete the servo task in the presence of input and output constraints, greatly alleviate the calculation pressure, and improve the servo accuracy and the accuracy of the controller calculation.

Description

Technical Field

[0001] The invention belongs to the field of mechanical arm visual servoing, and in particular relates to a visual servo control method based on event triggering model prediction. Background Art

[0002] Robotic arm visual servo is a servo control system that relies solely on the image information of feature points and the joint information of the robotic arm. The system is mainly divided into a visual system, usually a camera, and a robotic arm system. The traditional solution is to use image information and joint information to construct an image Jacobian matrix to design a PD control rate to complete the servo task. The characteristics of this solution are clear physical meaning, each parameter can find the corresponding physical parameters, the structure is simple, and the controller design is relatively concise. However, its parameters are strongly coupled, and there are great difficulties in parameter identification and control scheme design. In addition, the traditional PD control algorithm lacks constraint processing. In the actual visual servo system, the input end is usually the joint torque input of the robotic arm, which is subject to the physical limitations of the actual motor and has upper and lower limits; the output end is usually the feature point information of the pixel plane. The camera's field of view has a range. If it is not constrained, the target point may exceed the field of view, resulting in the failure of the servo task.

[0003] In addition to the above problems, another major shortcoming of visual servoing is that it requires too much computing power from the computing platform. The computing power pressure of the visual servo control system is mainly reflected in 1. The forward and inverse solutions of the dynamics or kinematics in the robotic arm system itself require a large number of matrix operations; 2. Visual servoing is based on feature point matching, and the feature point positioning and matching algorithm itself also has high computing power requirements. 3. Controller algorithm and parameter estimation, in order to improve accuracy, real-time parameter estimation is often performed on the controlled object, which also involves a large number of differential equations and multivariate equations. In actual systems, the computing platform is limited by hardware and cannot provide unlimited computing power. If the control problem cannot be solved in time and input information cannot be sent to the controlled object, the control accuracy may decrease or even the servo task may fail.

[0004] In summary, the existing visual servo control system for robotic arms lacks the processing of constraints and the corresponding measures for high computing pressure, which will challenge the overall performance of the system. Therefore, it is of great significance to design a suitable visual servo control method to solve the above problems. Summary of the invention

[0005] In view of the above technical problems existing in the prior art, the present invention proposes a visual servo control method based on event-triggered model prediction, which has a reasonable design, solves the shortcomings of the prior art and has good effects.

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

[0007] A visual servo control method based on event-triggered model prediction includes the following steps:

[0008] S1. Construct a visual servo system structure, including a robotic arm, a camera, and a target object, and construct a depth-independent model as the controlled object based on image information and joint information of the robotic arm;

[0009] S2. Design a model predictive controller using a depth-independent model to predict the optimal input and optimal state sequence under specified constraints;

[0010] S3, designing an event triggering mechanism based on the deviation information between the image information and the optimal state generated by the model predictive controller;

[0011] S4. The model prediction controller is controlled by the event triggering mechanism. If the event triggering conditions are met, the model prediction controller issues control instructions to the robot arm to control the movement of the joint. If the servo task is not completed, the feature point matching information and joint information are used to estimate the system parameters and the three-dimensional position information of the feature points in real time, and update the depth-independent model.

[0012] Furthermore, in S1, y(t)=(u,v) T is the coordinate of the feature point in the pixel plane, c z(t) represents the depth of the feature point in the camera coordinates, and the depth-independent model is as follows:

[0013]

[0014] Where t is the continuous time, is the joint velocity of the robot arm, which is the input of the depth-independent model, and Due to the separation from the depth information, A(t) is a depth-independent matrix, and its expression is as follows:

[0015]

[0016] in, is the three-dimensional coordinate of the feature point in the base coordinate system, specifically expressed as b x=(x,y,z) T ; is the product of the internal parameter matrix and the external parameter matrix, is the i-th row of M, is the homogeneous transformation matrix from the robot base coordinate system to the camera coordinate system. The joint angles of the two degrees of freedom are q1 and q2, and the connecting rod lengths are l1 and l2. Therefore It is expressed as follows:

[0017]

[0018] Furthermore, in S2, the depth-independent model is discretized as:

[0019]

[0020] Where k is the discrete time, x(k) is the selected state variable, which represents the coordinates of the feature point in the pixel plane in this system, T s It is represented as sampling time, u(k) is represented as input, that is, joint velocity

[0021] The core problem of model predictive control is to solve an optimal control problem, the goal of which is to minimize the cost function, namely:

[0022]

[0023] Where N is the prediction interval of the model predictive controller, and the expression of the cost function is as follows:

[0024]

[0025] Among them, e(k+N|k) T Fe(k+N|k) represents the terminal error, represents the servo error, e(k+j|k)=y(k+j|k)-y d ,y d is the desired feature point trajectory or coordinates, u(k+j|k) T Su(k+j|k) represents the input energy; matrices F, G, and S represent the weights of each item;

[0026] The input and output of the visual servo control system are restricted as follows:

[0027] u min ≤u(k+j|k)≤u max ,j=0,...,N-1; (7)

[0028]

[0029] y min ≤y(k+j|k)≤y max ,j=0,...,N-1; (9)

[0030] Model predictive control mainly uses model information to predict future states, and iterates formula (4) to obtain:

[0031]

[0032] Substitute formula (10) into formula (6) and simplify it to express it in quadratic form:

[0033]

[0034] Combining formulas (5), (7), (8), (9) and (11), we construct an optimization problem and use gradient descent optimization to obtain the optimal control sequence and the optimal state sequence, namely:

[0035] U * (k)=[U * (k+1|k) U * (k+2|k) … U * (k+N|k)] T ; (12)

[0036] X * (k) = [X * (k+1|k) X * (k+1|k) … X * (k+N|k)] T ; (13)

[0037] Then, combined with the characteristics of model predictive control rolling optimization, the first item of the optimal control sequence is taken as the input, that is, the optimal input, and its expression is:

[0038] S * (k) = U * (k+1|k) (14).

[0039] Furthermore, in S3, the event triggering mechanism is designed as follows:

[0040] y e =||y * (k|k-1)-y(k)||; (15)

[0041] Among them, y e It is the difference between the first item of the optimal output sequence generated by the controller at the previous moment and the actual output at the current moment;

[0042] If y e If it is greater than the trigger threshold α, the model predictive controller works so that u(k)=S * (k);

[0043] If y e If it is less than the trigger threshold α, the model predictive controller does not work, so that:

[0044]

[0045] t k+1= inf{t>t k |||y e ||≥α}; (17)

[0046] Among them, t k is the current triggering moment, t k+1 For the next trigger moment.

[0047] Furthermore, in S4, the unknown system parameters are first identified:

[0048]

[0049] By separating the known system parameters and the unknown system parameters, we get:

[0050] y(k)=ξ T ψ; (19)

[0051]

[0052] According to formula (19), ψ is solved, which is the sum of M and b x=(x,y,z) T The system parameters composed of various elements, including 38 system parameters in total;

[0053] Secondly, the depth of the feature points is estimated using the following formula:

[0054]

[0055] The above formula can be separated into the following forms:

[0056] c z(k)=η T δ; (22)

[0057] Among them, η T is generated by the joint angles q1 and q2 is the set of all elements in ; δ is a subset of ψ, including 13 system parameters in total;

[0058] The depth information is obtained by solving formula (22): c z(k); After using the Kalman filter algorithm, through the system parameters ψ and depth information c z(k) is then used to update the depth-independent model, i.e., formula (4).

[0059] The beneficial technical effects brought by the present invention are:

[0060] The present invention provides a visual servo control method based on event-triggered model prediction. The model prediction control algorithm and event triggering scheme are constructed respectively through feature point image information and robot arm joint information, thereby realizing the control of the robot arm visual servo system. Its beneficial effects include: 1) a constraint-based model prediction controller for the robot arm is designed, which can complete the servo task under the condition of input and output constraints. 2) a visual servo control scheme based on event triggering mechanism is designed, which can greatly alleviate the computing pressure. 3) in this system, parameter estimation and depth estimation information are added to improve the accuracy of servo and the accuracy of controller calculation. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a schematic diagram of the structure of the visual servo system in the present invention;

[0062] Figure 2 It is a schematic diagram of the pixel plane feature point route in the present invention;

[0063] Figure 3 It is a flow chart of the visual servo control system in the present invention;

[0064] Figure 4 This is a working principle diagram of the model predictive controller in the present invention; DETAILED DESCRIPTION

[0065] The present invention proposes a visual servo control method based on event-triggered model prediction. In order to make the advantages and technical solutions of the present invention clearer and more specific, the present invention is described in detail below in conjunction with specific embodiments.

[0066] A visual servo control method based on event-triggered model prediction includes the following steps:

[0067] S1. Construct a depth-independent model as the controlled object based on image information and joint information of the robotic arm;

[0068] Construct a visual servo system structure, such as Figure 1 As shown, it reflects the physical structure of the system. It is mainly divided into three parts. The first part is the robot arm part, that is, the main execution part and the controlled part. After the controller generates an input command, the robot arm specifically performs motion operations to complete the servo task. The second part is the perception part. The present invention uses a camera. The camera collects images of objects in space and transmits them to the computing platform for processing. It is mainly necessary to extract and locate the feature points of the collected image. After positioning, it is necessary to match the feature points of the expected image with the current image to construct the task function and control rate. The third part is the target object, a three-dimensional object in space. The object information needs to be collected by the camera to construct feature points. The object can be static or moving.

[0069] The system needs to initialize the system parameters, so first of all, we need to use the matching information of the feature points and the joint information of the robot to build a depth-independent matrix as the controlled model. The specific form of this model is the Jacobian form of the motion speed of the feature points and the joint speed of the robot. It is mainly a process of processing the combination of the camera projection model and the robot kinematic model.

[0070] Let y(t)=(u,v) T is the coordinate of the feature point in the pixel plane, c z(t) represents the depth of the feature point in the camera coordinates, and the depth-independent model is as follows:

[0071]

[0072] Where t is the continuous time, is the joint velocity of the robot arm, which is the input of the depth-independent model, and Due to the separation from the depth information, A(t) is a depth-independent matrix, and its expression is as follows:

[0073]

[0074] in, is the three-dimensional coordinate of the feature point in the base coordinate system, specifically expressed as b x=(x,y,z) T ; is the product of the internal parameter matrix and the external parameter matrix, is the i-th row of M, is the homogeneous transformation matrix from the robot base coordinate system to the camera coordinate system. The joint angles of the two degrees of freedom are q1 and q2, and the connecting rod lengths are l1 and l2. Therefore It is expressed as follows:

[0075]

[0076] S2. Design a model predictive controller using a depth-independent model to predict the optimal input and optimal state sequence under specified constraints;

[0077] Figure 2 It is the representation of the visual servoing task in the pixel plane. After the feature point positioning and matching, the position information of the current feature point in the pixel plane and the coordinate information of the point expected by the servo task can be obtained. The coordinates of the expected point can be either static or dynamic. The controller is usually designed by taking the deviation value of the two coordinates as the input of the controller. In the model prediction controller, different weights can be set and different paths can be generated through preferences to make the feature point move from the initial point to the expected point.

[0078] Figure 3 This is a flow chart of the visual servo control system based on model predictive control, which reflects the execution order and logic of the servo task. At the beginning, the system needs to initialize the initial parameters, such as the internal and external parameters of the camera, the initial angle of the joint, etc. After the initialization is completed, the system needs to locate and match the feature points of the image captured by the camera, so as to obtain the position of the current feature point and the position of the expected feature point. After the positioning is completed, the model predictive controller is designed based on the initialized model parameters and image information. After the controller completes the optimization problem, it will issue a control instruction to the robot arm, that is, send control input information to control the rotation of the joint motor of the robot arm, thereby controlling the movement of the joint and making the camera mounted at the end of the robot arm move. After completing a cycle of movement, the camera is used to determine whether the feature point moves to the expected position in the image plane and whether to jump out of the loop. If the servo task is not completed, since the depth-independent matrix is ​​a time-varying matrix, it is necessary to update the model through parameter identification and depth estimation to improve the accuracy of the servo task.

[0079] Since the model used by the model predictive controller is usually a discrete model, it is necessary to discretize formula (1). Since the set system has limitations, the system does not have an infinite slope, which can satisfy the Lipschitz condition. The discretization of the depth-independent model is expressed as:

[0080]

[0081] Where k is the discrete time, x(k) is the selected state variable, which represents the coordinates of the feature point in the pixel plane in this system, T s It is represented as sampling time, u(k) is represented as input, that is, joint velocity

[0082] The core problem of model predictive control is to solve an optimal control problem, the goal of which is to minimize the cost function, namely:

[0083]

[0084] Where N is the prediction interval of the model predictive controller, and the expression of the cost function is as follows:

[0085]

[0086] Among them, these three items represent the three parameters concerned by the visual servo control system, e(k+N|k) T Fe(k+N|k) represents the terminal error, which mainly reflects the system's large-scale convergence capability; represents the servo error, which represents the dynamic tracking capability of the system, e(k+j|k)=y(k+j|k)-yd ,y d is the expected feature point trajectory or coordinates; u(k+j|k) T Su(k+j|k) represents the energy at the input end, which reflects the energy of the motor at the input end; matrices F, G, and S represent the weights of each item. By adjusting the weights, the indicators of interest can be obtained;

[0087] In the visual servo control system, there are limitations on the actual input and output. The input is the motor speed and acceleration. In practice, these two data have upper and lower limits. On the output side, the camera's field of view has a fixed value. If the target moves too fast or the robot moves too fast, the target may exceed the field of view, resulting in mission failure. Limitations are imposed on the input and output of the visual servo control system as follows:

[0088] u min ≤u(k+j|k)≤u max ,j=0,...,N-1; (7)

[0089]

[0090] y min ≤y(k+j|k)≤y max ,j=0,...,N-1; (9)

[0091] Model predictive control mainly uses model information to predict future states, and iterates formula (4) to obtain:

[0092]

[0093] Substitute formula (10) into formula (6) and simplify it to express it in quadratic form:

[0094]

[0095] Combining formulas (5), (7), (8), (9) and (11), we construct an optimization problem and use gradient descent optimization to obtain the optimal control sequence and the optimal state sequence, namely:

[0096] U * (k)=[U * (k+1|k) U * (k+2|k) … U * (k+N|k)] T ; (12)

[0097] X * (k) = [X * (k+1|k) X *(k+1|k) … X * (k+N|k)] T ; (13)

[0098] Then, combined with the characteristics of model predictive control rolling optimization, the first item of the optimal control sequence is taken as the input, that is, the optimal input, and its expression is:

[0099] S * (k) * (k) = U * (k+1|k) (14).

[0100] S3, designing an event triggering mechanism based on the deviation information between the image information and the optimal state generated by the model predictive controller;

[0101] The main controller is a model predictive controller, which calculates the optimal control input sequence and state sequence and provides the optimal input S to the control model. * (k) Control. The event trigger mechanism mainly uses the MPC characteristics to subtract the output signal generated by the real model from the output signal of the prediction model as the judgment condition for event triggering. If the difference is greater than the set threshold, the MPC controller will solve the OCP through calculation and obtain the optimal control input S * (k) and then control the movement of the servo system; if the condition is not met, the MPC controller will not perform calculations, but will command the input system to maintain the input status of the previous sampling interval, thereby reducing the amount of calculations. In the time interval between two trigger events, a zero-order holder is used to maintain the input. The physical meaning of the trigger condition of this system is clear. The output difference between the real model and the predicted model represents the total error of the entire system, which includes the error at the input torque end and the total error of the matching of the image feature points at the output end. In addition, its structure is simple and the parameters can be adjusted conveniently. The threshold can be adjusted according to the required number of triggers and error conditions.

[0102] The designed event trigger mechanism is:

[0103] y e =||y * (k|k-1)-y(k)||; (15)

[0104] Among them, y e It is the difference between the first item of the optimal output sequence generated by the controller at the previous moment and the actual output at the current moment;

[0105] If y e If it is greater than the trigger threshold α, the model predictive controller works so that u(k)=S * (k);

[0106] If ye If it is less than the trigger threshold α, the model predictive controller does not work, so that:

[0107]

[0108] t k+1 = inf{t>t k |||y e ||≥M}; (17)

[0109] Among them, t k is the current triggering moment, t k+1 is the next trigger moment. Formula (16) indicates that between two trigger moments, the system will maintain the optimal input generated by the controller at the previous trigger moment through the zero-order holder;

[0110] S4. The model prediction controller is controlled by the event triggering mechanism. If the event triggering conditions are met, the model prediction controller issues control instructions to the robot arm to control the movement of the joint. If the servo task is not completed, the feature point matching information and joint information are used to estimate the system parameters and the three-dimensional position information of the feature points in real time, and update the depth-independent model.

[0111] Figure 4 The control system block diagram after completing all the processes reflects the signal flow of the entire system. The model predictive controller is controlled by the event trigger mechanism. If the event triggering condition is met, that is, the error exceeds the threshold, the controller works to generate a new control command; if it is less than the threshold, the error is within the specified range, the controller does not work, and the joint motor of the robot arm maintains the control command of the last trigger time. The event triggering condition is the deviation between the predicted state sequence generated by the controller and the current position of the image feature point. In order to reduce the fluctuation of system parameters and improve the accuracy of the system, visual servo control often requires parameter identification and depth estimation of the system, and uses the estimated parameter information to update the model part of the model predictive control, so that it can adapt well to environmental changes even in the case of disturbance, thereby having a better servo effect.

[0112] In S4, the unknown system parameters are first identified:

[0113]

[0114] By separating the known system parameters and the unknown system parameters, we get:

[0115] y(k)=ξ T ψ; (19)

[0116]

[0117] According to formula (19), ψ is solved, which is the sum of M and b x=(x,y,z) T The system parameters composed of various elements, including 38 system parameters in total;

[0118] Secondly, the depth of the feature points is estimated using the following formula:

[0119]

[0120] The present invention assumes that the joint angles q1, q2 and the feature point position y(t) are known, and the above equation is subjected to parameter separation into the following form:

[0121] c z(k)=η T δ; (22)

[0122] Among them, η T is generated by the joint angles q1 and q2 is the set of all elements in ; δ is a subset of ψ, including 13 system parameters in total;

[0123] The depth information is obtained by solving formula (22): c z(k); After using the Kalman filter algorithm, through the system parameters ψ and depth information c z(k) is then used to update the depth-independent model, i.e., formula (4).

[0124] 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 visual servo control method based on event-triggered model prediction, characterized in that: The following steps are involved: S1. Construct a visual servo system structure, including a robotic arm, a camera, and a target object, and construct a depth-independent model as the controlled object based on image information and joint information of the robotic arm; S2. Design a model predictive controller using a depth-independent model to predict the optimal input and optimal state sequence under specified constraints; S3, designing an event triggering mechanism based on the deviation information between the image information and the optimal state generated by the model predictive controller; S4, the model prediction controller is controlled by the event trigger mechanism. If the event triggering conditions are met, the model prediction controller issues a control instruction to the robot arm to control the movement of the joint; if the servo task is not completed, the feature point matching information and joint information are used to estimate the system parameters and the three-dimensional position information of the feature points in real time, and update the depth-independent model; In S1, let y(t) = (u, v) T is the coordinate of the feature point in the pixel plane, cz(t) represents the depth of the feature point in the camera coordinate, and the depth-independent model is as follows: Where t is the continuous time, is the joint velocity of the robot arm, which is the input of the depth-independent model, and Due to the separation from the depth information, A(t) is a depth-independent matrix, and its expression is as follows: in, is the three-dimensional coordinate of the feature point in the base coordinate system, specifically expressed as b x=(x,y,z) T ; is the product of the internal parameter matrix and the external parameter matrix, is the i-th row of M, is the homogeneous transformation matrix from the robot base coordinate system to the camera coordinate system. The joint angles of the two degrees of freedom are q1 and q2, and the connecting rod lengths are l1 and l2. Therefore It is expressed as follows: In S2, the depth-independent model is discretized as: Where k is the discrete time, x(k) is the selected state variable, which represents the coordinates of the feature point in the pixel plane in this system, T s It is represented as sampling time, u(k) is represented as input, that is, joint velocity The core problem of model predictive control is to solve an optimal control problem, the goal of which is to minimize the cost function, namely: Where N is the prediction interval of the model predictive controller, and the expression of the cost function is as follows: Among them, e(k+N|k) T Fe(k+N|k) represents the terminal error, represents the servo error, e(k+j|k)=y(k+j|k)-y d ,y d is the desired feature point trajectory or coordinates, u(k+j|k) T Su(k+j|k) represents the input energy; matrices F, G, and S represent the weights of each item; The input and output of the visual servo control system are restricted as follows: u min ≤u(k+j|k)≤u max ,j=0,…,N-1; (7) y min ≤y(k+j|k)≤y max ,j=0,…,N-1; (9) Model predictive control mainly uses model information to predict future states, and iterates formula (4) to obtain: Substitute formula (10) into formula (6) and simplify it to express it in quadratic form: Combining formulas (5), (7), (8), (9) and (11), we construct an optimization problem and use gradient descent optimization to obtain the optimal control sequence and the optimal state sequence, namely: YOU * (k)=[You * (k+1|k) U * (k+2|k)…U * (k+N|k)] T ; (12) X * (k)=[X * (k+1|k) X * (k+1|k)…X * (k+N|k)] T ; (13) Then, combined with the characteristics of model predictive control rolling optimization, the first item of the optimal control sequence is taken as the input, that is, the optimal input, and its expression is: S * (k)=U * (k+1|k); (14) In S3, the designed event trigger mechanism is: and e =||and * (k|k-1)-y(k)||; (15) Among them, y e It is the difference between the first item of the optimal output sequence generated by the controller at the previous moment and the actual output at the current moment; If y e If it is greater than the trigger threshold α, the model predictive controller works so that u(k)=S * (k); If y e If it is less than the trigger threshold α, the model predictive controller does not work, so that: t k+1 =inf{t>t k |||y e ||≥α}; (17) Among them, t k is the current triggering moment, t k+1 For the next trigger moment; In S4, the unknown system parameters are first identified: By separating the known system parameters and the unknown system parameters, we get: y(k)=ξ T ψ; (19) According to formula (19), ψ is solved, which is the sum of M and b x=(x,y,z) T The system parameters composed of various elements, including 38 system parameters in total; Secondly, the depth of the feature points is estimated using the following formula: The above formula can be separated into the following forms: c z(k)=η T d;(22) Among them, η T is generated by the joint angles q1 and q2 is the set of all elements in ; δ is a subset of ψ, including 13 system parameters in total; The depth information is obtained by solving formula (22): c z(k); After using the Kalman filter algorithm, through the system parameters ψ and depth information c z(k) is then used to update the depth-independent model, i.e., formula (4).

Images