A mechanical arm control method for automatic docking of static targets

Abstract

The present application relates to the technical field of mechanical arm target docking, and specifically provides a mechanical arm control method for automatic docking of static targets, which cascades a force prediction control system and a static visual prediction control system, the force prediction control system outputs a variation of moment features of real-time images according to the sensing of actual external force on the end of the mechanical arm by a force sensor on the mechanical arm, acquires target images at different times by a camera arranged at the end of the mechanical arm, and calculates a visual feature error in combination with the variation of moment features of real-time images, inputs the visual feature error into the static visual prediction control system to obtain a first element of a predicted input variable, calculates the joint speed of the mechanical arm according to the first element, and further realizes the motion control of the mechanical arm. The control method of the present application cascades the force prediction control system and the static visual prediction control system, improves the environmental sensing ability and interaction ability of the mechanical arm, and can effectively improve the effectiveness, reliability and accuracy of the docking task.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm target docking, and in particular provides a robot arm control method for automatic docking of static targets. Background Art

[0002] Automatic docking refers to the process in which a robotic arm accurately and stably connects or coordinates target objects or systems through precise operation and control without human intervention. Automatic docking technology has been widely researched and applied in many fields such as industrial manufacturing, aerospace, marine engineering, space exploration, etc., and is promoting the development of intelligence and automation in many industries. Automatic docking at the end of the robotic arm requires high-precision positioning and control as well as real-time perception and decision-making to cope with the variability of the environment and targets, and to ensure high accuracy and high safety of the docking task. At present, the main methods for implementing automatic docking tasks at the end of the robotic arm under static targets include the following:

[0003] Visual recognition: Use cameras or other visual sensors to identify the target object, determine its position and posture, and then control the joints and end effector of the robotic arm to dock with the target object.

[0004] Sensor detection: Proximity sensors, force sensors, etc. are used to detect changes in the distance and force between the end of the robotic arm and the target object, thereby achieving docking tasks.

[0005] Laser ranging: Use a laser sensor to measure the distance to the target object, and then control the end effector of the robotic arm to accurately dock with the target object.

[0006] Precision control algorithm: Through advanced control algorithms, combined with sensor feedback information, precise control of the end of the robotic arm is achieved, thereby completing the docking task.

[0007] Currently, there are many shortcomings in the automatic docking task of the end-of-arm manipulator under static targets:

[0008] 1. High cost: Introducing technologies such as visual recognition, sensor detection, and laser ranging requires a high investment, which may be unaffordable, especially for small businesses or individual users.

[0009] 2. High technical requirements: These methods require a high degree of technical support and professional knowledge, and have high technical requirements for operators, requiring professional training and experience accumulation.

[0010] 3. Strict environmental requirements: For technologies such as visual recognition and laser ranging, environmental factors such as light, temperature, and humidity will affect their accuracy, requiring stricter environmental control.

[0011] 4. Reliability: The accuracy and stability of the sensor are crucial to the success of the automatic docking mission. Sensor failure or inaccuracy may lead to docking failure.

[0012] 5. Complexity: The entire automatic docking system may become complicated due to the combination of multiple components, requiring more maintenance and debugging.

[0013] 6. Limited scope of application: Different methods are suitable for different scenarios, and some methods may not be applicable to specific environments or tasks.

[0014] In order to solve the above problems, it is necessary to propose a new robotic arm control method for static target docking. Summary of the Invention

[0015] In order to solve the above problems, the present invention provides a robotic arm control method for automatic docking of static targets. The control system corresponding to this control method cascades a force prediction control system (FPC) and a static vision prediction control system (SVPC). The FPC in the control system can improve the interaction ability of the robotic arm with the external environment, and the SVPC can improve the robotic arm's perception of the environment. The cascade of the two can improve the flexibility, adaptability, control performance and execution efficiency of the robotic arm, thereby improving the effectiveness, reliability and accuracy of the robotic arm's automatic docking task.

[0016] The present invention provides a robotic arm control method for automatic docking of static targets, comprising:

[0017] S1: Set up a camera on the robotic arm and a force sensor on the end effector of the robotic arm;

[0018] Set multiple feature points, connect them in sequence to form a target, and the position of the target is fixed relative to the static target;

[0019] S2: dock the end effector of the manipulator with the static target, capture the target image of the target through the camera, and obtain the expected image moment feature S of the target in the target image d ;

[0020] S3: During the docking process between the end effector of the manipulator and the static target, the camera collects the target image in real time and obtains the real-time image moment feature S of the target through the real-time target image;

[0021] S4: Sense the actual external force f on the end effector of the robotic arm through the force sensor ext , the actual external force f in the Cartesian coordinate system ext and the expected external force f ext d The error is converted into the unit inertia virtual force error F in the image feature space s e :

[0022]

[0023] Among them, M c * represents the desired inertia matrix of the camera, Represents the transformation matrix from the camera coordinate system to the robot end coordinate system, L s Represents the image Jacobian matrix corresponding to the real-time image moment feature S;

[0024] S5: Establish the state space equation of the force prediction control system:

[0025]

[0026] Among them, F s e (k) represents the state variable of the force prediction control system at the current control step, F s e (k+1) represents the state variable of the force prediction control system for the next control step, S * (k) represents the control input variable of the force prediction control system, Y f (k) represents the control output variable of the force prediction control system, T represents the sampling period, M = -H, H represents the positive definite gain matrix, k s represents the unit inertial stiffness matrix;

[0027] S6: The state space equation of the force prediction control system is transformed into a standard form for solving the optimization problem through the quadratic programming method:

[0028]

[0029] Among them, J c represents the loss function of the force predictive control system, S *′ k It represents the predicted input variable of the force predictive control system at the current control step, F s e k Indicates the current control step F s e The status value of represents the upper limit matrix of the inequality constraints of the force predictive control system, represents the lower bound matrix of the inequality constraints of the force predictive control system, B f The extended state vector F represents the force predictive control system s e′ k With S *′k The control matrix of the relationship between f Represents the weight matrix of the error term of the force predictive control system, R f Represents the weight matrix of the input items of the force prediction control system, A represents F s e′ k With S *′ k The state matrix of the relationship between f The weight matrix of the terminal error term of the force predictive control system, D f The coefficient matrix representing the inequality constraints of the force predictive control system, A f The coefficient matrix representing the equality constraints of the force predictive control system, b f Represents the equality constraint matrix of the force predictive control system;

[0030] S7: The unit inertia virtual force error F s e Input the standard form in S6 to obtain the predicted input variable S of the force prediction control system *′ k , the predicted input variable S *′ k The first element is the change S of the real-time image moment feature S * , calculate the reference image moment feature S r :

[0031] S r =S d -S * ;

[0032] Calculate the visual feature error S e :

[0033] S e =SS r ;

[0034] S8: Establish the state space equation of static visual predictive control system:

[0035]

[0036] Among them, S(k) represents the state variable of the static visual predictive control system of the current control step, S(k+1) represents the state variable of the static visual predictive control system of the next control step, and v c (k) represents the control input variable of the static visual predictive control system, Y s (k) represents the control output variable of the static visual predictive control system, Represents the expected image moment feature S d The corresponding image Jacobian matrix;

[0037] S9: The state space equations of the static visual predictive control system are transformed into a standard form for solving the optimization problem through the quadratic programming method:

[0038]

[0039] Among them, J c represents the loss function of the static visual predictive control system, represents the upper bound matrix of the inequality constraints of the static visual predictive control system, represents the lower bound matrix of the inequality constraints of the static visual predictive control system, represents the predicted input variable of the static visual predictive control system, S e k Represents the current control step visual feature error S e The status value of B s express and The control matrix of the relationship between represents the state error predictor variable, Q s Represents the weight matrix of the error term of the static visual predictive control system, R s Represents the weight matrix of the static visual predictive control system input, T s represents the weight matrix of the terminal error term of the static visual predictive control system, and A represents and The state matrix of the relationship between s The coefficient matrix A represents the inequality constraints of the static visual predictive control system. s The coefficient matrix representing the equality constraints of the static visual predictive control system, b s represents the equality constraint matrix of the static visual predictive control system;

[0040] S10: The visual feature error S e Enter the standard form in S9 to obtain the predicted input variables of the static visual predictive control system Enter the prediction variable The first element is the camera speed v C , according to the camera speed v C Calculating the joint velocity of the robotic arm Control the robotic arm according to the joint speed Do some exercise.

[0041] Preferably, based on the target image, the expected image moment feature S is calculated d The expressions of the real-time image moment feature S are:

[0042] [xn ,y n , a n ,θ,γ,α] T ;

[0043] Expected image moment feature S d The calculation method of the real-time image moment feature S is:

[0044]

[0045] m pq =∫∫ O x p y q f(x,y)dxdy;

[0046] μ ij =∫∫ O (xx g ) i (yy g ) j f(x,y)dxdy;

[0047] Among them, a * represents the expected area of ​​the target in the target image, a represents the actual area of ​​the target in the target image, and Z * Indicates the depth between the camera and the target, m pq represents the continuous origin moment of the target image, O represents the image plane of the target image, f(x, y) represents the grayscale value of the pixel with coordinates (x, y) in the target image, μ ij Represents the continuous center distance of the target image, (x g =m 10 / m 00 ,y g =m 01 / m 00 ) represents the coordinates of the center of gravity of the target image.

[0048] Preferably, the image feature space is the image moment feature [x n ,y n , a n ,θ,γ,α] T The corresponding space.

[0049] Preferably, the unit inertia virtual force error F s e The solution is:

[0050] A robotic arm model without inertia is established and converted into the image feature space as follows:

[0051]

[0052] in,

[0053]

[0054] J s represents the Jacobian matrix of the image feature space, M(q) represents the inertia matrix of the manipulator, represents the Coriolis force matrix, g(q) represents the gravity term, τ represents the torque output term of the manipulator joint, J T f ext Indicates the torque term that maps the actual external force to the robotic arm joint, Represents the unit inertia virtual force projected into the image feature space by the actual external force, f s represents the unit inertia virtual force, j represents the first-order derivative of J;

[0055] The desired inertia matrix M of the camera c * Calculate the unit inertia virtual force F s :

[0056]

[0057] Preferably, the method for establishing the state space equation of the force prediction control system is: establishing the state space equation of the force prediction control system by introducing a positive definite gain matrix H in the visual admittance control in the image feature space.

[0058] Preferably, the loss function of the force prediction control system is:

[0059]

[0060] Among them, F s e (k+N|k) represents the unit inertia virtual force error corresponding to the k+Nth control step predicted by the current control step, F s e (k+i|k) represents the unit inertia virtual force error corresponding to the k+i control step predicted by the current control step, S * (k+i|k) represents the predicted input variable of the force predictive control system corresponding to the k+i-th control step predicted by the current control step.

[0061] Preferably, the loss function of the static visual predictive control system is:

[0062]

[0063] Among them, S e (k+N|k) represents the visual feature error corresponding to the k+Nth control step predicted by the current control step, S e(k+i|k) represents the visual feature error corresponding to the k+i control step predicted by the current control step, v C (k+i|k) represents the predicted input variable of the static visual predictive control system corresponding to the k+i control step predicted by the current control step.

[0064] Preferably, the robot arm joint speed The calculation method is:

[0065] According to the camera speed v C , through the transformation matrix from the robot end coordinate system to the camera coordinate system Calculate the end velocity v of the robot arm e , according to the end velocity v of the robot arm e , through the inverse matrix J of the Jacobian matrix of the manipulator -1 Calculating the joint velocity of the robotic arm

[0066] Compared with the prior art, the present invention can achieve the following beneficial effects:

[0067] The control system provided by the present invention cascades a force prediction control system and a static visual prediction control system, wherein the force prediction control system can improve the interaction ability of the robot arm with the external environment, and the static visual prediction control system can improve the robot arm's ability to perceive the environment. Compared with a single visual recognition or sensor detection method, the control system formed by the cascade of the two integrates the information obtained by the camera and the force sensor, which can improve the docking control accuracy and perception ability of the robot arm when completing the static docking task. In addition, the present invention does not require expensive components, effectively reducing costs, has low requirements for environmental temperature and humidity, is easy to implement, has a wide range of uses, and is highly adaptable. It improves the robot arm's ability to perceive the environment and its interaction ability, and can effectively improve the effectiveness, reliability, and accuracy of the docking task. At the same time, the cascaded control system of the present invention introduces a variety of constraints, so that the control system will not exceed the specified constraint range during movement, thereby ensuring the safety and stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 is a schematic diagram of a cascade connection of a force prediction control system and a static vision prediction control system provided in an embodiment of the present invention;

[0069] Figure 2 This is a schematic diagram of an actual situation in which a robotic arm completes a static target docking task according to an embodiment of the present invention. DETAILED DESCRIPTION

[0070] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the following description, identical modules are denoted by identical reference numerals. In the case of identical reference numerals, their names and functions are also identical. Therefore, their detailed description will not be repeated.

[0071] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and do not constitute a limitation of the present invention.

[0072] like Figure 1 As shown, an embodiment of the present invention provides a robotic arm control method for automatic docking of static targets. The control system of the control method cascades two different model predictive control (MPC) systems, namely, a force predictive control system (FPC) and a static vision predictive control system (SVPC), and specifically includes the following steps:

[0073] S1: A camera and force sensor are installed on the robotic arm. The camera is fixed relative to the robotic arm and can be mounted on the robotic arm's end effector to follow the movement of the robotic arm. The force sensor is installed on the robotic arm's end effector and follows the movement of the robotic arm. The force sensor can sense the actual external force applied to the robotic arm's end effector in real time.

[0074] A plurality of feature points are randomly set, and the feature points are connected in sequence to form a pattern, which is called a target. The target must meet the following requirements: the target position is fixed relative to the static target, and an area of ​​the plane where the static target is located is usually selected as the target. The size, area and other factors of the target are irrelevant to the type of docking task performed by the robot arm. The target can be any complete, characteristic, and clearly positioned graphic area. In an embodiment of the present invention, four feature points are set, and the four feature points are connected in sequence to form a rectangle, which is used as the target. The rectangle is directly below the static target, and the distance between the rectangle and the static target is known and relatively fixed.

[0075] Alternatively, you can first select a pattern as a target, which also needs to meet the above target requirements. After determining the target, select multiple points on the target as feature points.

[0076] During the docking process between the end effector of the robotic arm and the static target, the camera can capture the target in real time to obtain the target image.

[0077] S2: docking the end effector of the manipulator with the static target. The docking method can be to manually pull the manipulator to dock the end effector with the static target, or to remove the static target from the installation position and dock it with the end effector of the manipulator. In the docked state, the target image is captured by the camera, and the image moment feature of the target in this state is calculated, that is, the expected image moment feature S d , expected image moment feature S d It is the image moment feature corresponding to the robotic arm completing the docking task.

[0078] S3: Obtain the desired image moment feature S d After that, the robotic arm is separated from the static target again. During the docking process between the end effector of the robotic arm and the static target, the camera collects the target image of the target in real time. Based on the real-time target image, the real-time image moment feature S of the target in the target image is calculated.

[0079] Expected image moment feature S d The expressions of the real-time image moment feature S are

[0080] [x n ,y n , a n ,θ,γ,α] T ;

[0081] in,

[0082]

[0083] m pq =∫∫ O x p y q f(x,y)dxdy;

[0084] μ ij =∫∫ O (xx g ) i (yy g ) j f(x,y)dxdy;

[0085] a * represents the expected area of ​​the target in the target image, a represents the actual area of ​​the target in the target image, and Z * Indicates the depth between the camera and the target, m pq represents the continuous origin moment of the target image, p and q represent the powers of x and y respectively, O represents the image plane of the target image, f(x, y) represents the grayscale value of the pixel with coordinates (x, y) in the target image, μ ij Represents the continuous center distance of the target image, (x g =m10 / m 00 ,y g =m 01 / m 00 ) represents the coordinates of the center of gravity of the target image.

[0086] Component x n 、y n and a n Used to control the translation of the camera, that is, the linear velocity of the camera relative to the coordinate axis of the camera coordinate system (v x , v y ,v z ), the components θ, γ, and α are used to control the rotation of the camera, that is, the angular velocity of the camera relative to the coordinate axis of the camera coordinate system (w x , w y , w z ), the camera coordinate system is a preset coordinate system, which is preset by the user based on the actual experimental environment, task type and the robot arm's own conditions. The origin of the camera coordinate system is the camera's center of mass. The translation and rotation of the camera can be converted into the translation and rotation of the end of the robot arm, thereby controlling the movement of the robot arm.

[0087] S4: Perform inertia normalization on the traditional robotic arm model, remove the inertia term in the robotic arm model, and convert the processed robotic arm model into image moment features [x n ,y n , a n ,θ,γ,α] T In the corresponding space, which is referred to as the image feature space in the embodiment of the present invention, the following expression of the robotic arm model without inertia terms is obtained:

[0088]

[0089] in,

[0090]

[0091] J s represents the Jacobian matrix in the image feature space, M(q) represents the inertia matrix of the manipulator, represents the Coriolis force matrix, J represents the Jacobian matrix of the robot arm, represents the first-order derivative of J, Represents the transformation matrix from the robot arm end coordinate system to the camera coordinate system. The robot arm end coordinate system is a preset coordinate system, which is preset by the user based on the actual experimental environment, task type and robot arm itself. The origin of the robot arm end coordinate system is the center of mass of the robot arm end joint. s represents the image Jacobian matrix corresponding to the real-time image moment feature S, L s ∈R6×6 , g(q) represents the gravity term, τ represents the torque output term of the manipulator joint, f ext Represents the actual external force acting on the end effector of the robot arm, J T f ext represents f ext The torque terms mapped to the joints of the robotic arm, Represents the unit inertia virtual force projected into the image feature space by the actual external force, f s Represents the unit inertia virtual force;

[0092] In the robotic arm model under the image feature space The expression can be transformed into:

[0093]

[0094] in,

[0095]

[0096] M c represents the projection of the manipulator's inertia matrix in the camera coordinate system, Represents the projection of the actual external force in the camera coordinate system, Represents the transformation matrix from the robot end coordinate system to the camera coordinate system.

[0097] Define M c * is the expected inertia matrix of the camera, in the expected inertia matrix M of the camera c * Calculate the unit inertia virtual force F s :

[0098]

[0099] Among them, F s Indicates that in M c *-1 The unit inertia virtual force under .

[0100] In the Cartesian coordinate system, during the docking process between the end effector of the manipulator and the static target, the actual external force f on the end effector of the manipulator is ext and the expected external force f ext d There is an error between the actual external force and the actual external force f is obtained by the force sensor in real time sensing the actual external force on the end effector of the manipulator. ext Expected external force f ext d is a preset value, and its specific value is related to the docking task performed by the robot arm. For example, if the robot arm needs to complete the device docking, the expected external force fe xtd The best value is zero Newton; if the robot arm needs to complete the grinding and docking of the casting, assuming that the external force required for grinding is 80 Newtons, then the expected external force f ext d The best value of is 80 Newtons. ext and the expected external force f ext d The error between them is converted into the unit inertia virtual force error F in the image feature space s e :

[0101]

[0102] Among them, F s e =F s -F s d , F s d Represents the unit inertia virtual force in the image feature space.

[0103] S5: Admittance control is an existing force control system in the field of robotic arm control. It can ensure smoother and safer robotic arm movement. Admittance control adjusts the robotic arm's movement by measuring the actual external force information applied to the robotic arm. This control method can also be applied in the image feature space. By converting the existing admittance control into the image feature space, we obtain the visual admittance model:

[0104]

[0105] Among them, m s Indicates the unit inertial virtual mass, b s Represents unit inertial damping, k s represents the unit inertial stiffness matrix, S * Indicates the change of the real-time image moment feature S, S * =S d -S r , S d Represents the expected image moment feature of the feature point in the target image, S r Reference image moment features representing feature points in the target image.

[0106] S * =S d -S r Introduced into the visual admittance model, the following formula is obtained:

[0107]

[0108] In order to ensure that the force control system can respond quickly, a positive gain matrix H is introduced to ensure that Fs e It decreases exponentially. After introducing the positive definite gain matrix H, the following expression is obtained:

[0109]

[0110] During the docking process between the end effector of the manipulator and the static target, when the change of the real-time image moment feature S * When the value of does not change, the robot arm is said to be in a balanced state. and Converges to zero, and The value of is taken as 0 to simplify the visual admittance model. Introduced into the simplified visual admittance model, the unit inertia virtual force error derivative is obtained The change S with the real-time image moment feature S * The relationship is:

[0111]

[0112] Wherein, M=-H.

[0113] After the above calculations, the state space equation of the discrete-time force predictive control system (FPC) is obtained as follows:

[0114]

[0115] Among them, F s e (k) represents the state variable of the force prediction control system at the current control step, F s e (k+1) represents the state variable of the force prediction control system for the next control step, S * (k) represents the control input variable of the force prediction control system, Y f (k) represents the control output variable of the force prediction control system, and T represents the sampling period. The sampling period is the sampling period for the force sensor to collect the actual external force, and is also the period for extracting feature points from the target image obtained by the camera. The two periods are the same.

[0116] S6: Model Predictive Control (MPC) includes the prediction time domain N P and control time domain N c , prediction time domain N P Used to predict the time range of future system behavior and control the time domain N c The time range used to calculate the optimal control input. Let N P =N c = N, to simplify the control algorithm and reduce the control time domain, expand the state vector F se′ k The predicted input variable S of the force prediction control system at the current control step *′ k There are the following expressions:

[0117]

[0118] Among them, F s e (k+N|k) represents the unit inertia virtual force error F corresponding to the k+Nth control step predicted by the current control step s e , since the k+Nth control step is the last control step, so F s e (k+N|k) is also called the terminal unit inertia virtual force error predicted by the current control step, F s e (k|k)=F s e k Indicates the virtual force error F per unit inertia in the current control step s e The state value, S * (k|k) represents the state value of the predicted input variable of the force prediction control system corresponding to the current control step, S * (k+N-1|k) represents the predicted input variable of the force prediction control system corresponding to the k+N-1th control step predicted by the current control step, F s e (k+i|k) represents the unit inertia virtual force error F corresponding to the k+i control step predicted by the current control step s e , S * (k+i|k) represents the predicted input variable of the force predictive control system corresponding to the k+i-th control step predicted by the current control step.

[0119] Using the general method of constrained linear model predictive control, the loss function of the force predictive control system is set to obtain the optimal control variables. The loss function expression of the force predictive control system is as follows:

[0120]

[0121] Among them, Q f Represents the weight matrix of the error term of the force predictive control system, R f Represents the weight matrix of the input term of the force predictive control system, T f The weight matrix of the terminal error term of the force predictive control system, Q f 、R f and T fare all symmetric positive definite matrices.

[0122] According to the loss function of the force prediction control system, the current control step size F s e The state value F s e (k|k) and S * The relational expression of (k+i-1-j|k) is as follows:

[0123]

[0124] Among them, S * (k+i-1-j|k) represents the predicted input variable of the force predictive control system corresponding to the k+i-1-jth control step predicted by the current control step.

[0125] Further deduction yields:

[0126]

[0127] Among them, A represents F s e′ k With S *′ k The state matrix of the relationship between f Indicates F s e′ k With S *′ k The control matrix of the relationship between them, A∈R 6(N+1)×6 , B f ∈R 6(N+1)×6N , F s e k ∈R 6×1 , S *′ k ∈R 6N×1 .

[0128] The state space equation of the force predictive control system is transformed into an optimization problem to obtain the optimal control sequence. The state space equation of the force predictive control system is transformed into the standard form of the optimization problem through the quadratic programming method:

[0129]

[0130] in, represents the upper limit matrix of the inequality constraints of the force predictive control system, The lower bound matrix of the inequality constraints of the force predictive control system, D fThe coefficient matrix representing the inequality constraints of the force predictive control system, A f The coefficient matrix representing the equality constraints of the force predictive control system, b f Represents the equality constraint matrix of the force predictive control system.

[0131] S7: The standard form of solving the above optimization problem is Figure 1 The solvable expression of FPC shown in the figure is converted into the unit inertial virtual force error F in the image feature space. s e Input the standard form of the optimization problem of the force prediction control system to obtain the predicted input variable S of the force prediction control system *′ k , the first element of the predicted input variable is used as the change S of the real-time image moment feature S * , that is, the final output of the force prediction control system, and the change S * Feedback is given to the static visual prediction control system to achieve cascade.

[0132] According to the change S of the real-time image moment feature S * and the expected image moment feature S d Calculate the reference image moment feature S r :

[0133] S r =S d -S * .

[0134] According to the real-time image moment feature S and the reference image moment feature S r Calculate the visual feature error S e :

[0135] S e =SS r .

[0136] Visual feature error S e The solution process introduces the change S of the output value of the force prediction control system in real time image moment characteristic S * , and in the subsequent control process, the visual feature error S e yes Figure 1 The input values ​​of the static vision predictive control system shown in , therefore, it is possible to cascade the force predictive control system with the static vision predictive control system.

[0137] S8: The spatial state equation of the static vision predictive control system based on discrete time is as follows:

[0138]

[0139] Among them, S(k) represents the state variable of the static visual predictive control system of the current control step, S(k+1) represents the state variable of the static visual predictive control system of the next control step, and v c (k) represents the control input variable of the static visual predictive control system, Y s (k) represents the control output variable of the static visual predictive control system, Indicates that S d The corresponding target image Jacobian matrix.

[0140] S9: State prediction variable S of the static visual predictive control system with current control step k ′, state error prediction variable and predictor input variables The relationship expression is as follows:

[0141]

[0142] Where S(k|k)=S k represents the state prediction variable corresponding to the current control step of the static visual predictive control system, S(k+i|k) represents the state prediction variable corresponding to the k+i control step of the static visual predictive control system predicted by the current control step, S(k+N|k) represents the state prediction variable corresponding to the k+N control step of the static visual predictive control system predicted by the previous control step, v c (k|k) represents the predicted input variable of the static visual predictive control system corresponding to the current control step, v C (k+i|k) represents the predicted input variable of the static visual predictive control system corresponding to the k+i control step predicted by the current control step, v c (k+N-1|k) represents the predicted input variable of the static visual predictive control system corresponding to the k+N-1th control step predicted by the current control step, S e (k|k) represents the visual feature error S corresponding to the previous control step e , S e (k+N-1|k) represents the visual feature error S corresponding to the k+N-1th control step predicted by the current control step e , S e (k+N|k) represents the visual feature error S corresponding to the k+Nth control step predicted by the current control step e , since the k+i control step is the last control step of the robot, S e (k+N|k) is also called the terminal visual feature error predicted by the current control step, S e (k+i|k) represents the visual feature error S corresponding to the k+i control step predicted by the current control step e .

[0143] Based on the above relational expression, the loss function of the static visual predictive control system is designed as follows:

[0144]

[0145] Among them, Q s Represents the weight matrix of the error term of the static visual predictive control system, R s Represents the weight matrix of the static visual predictive control system input, T s represents the weight matrix of the terminal error term of the static visual predictive control system, Q s 、R s and T s are all positive definite symmetric matrices.

[0146] State prediction variable S(k|k), predicted visual feature error variable S e (k+i|k) and the predicted input variable v predicted by the current control step C The relationship between (k+i-1-j|k) is:

[0147]

[0148] and The relationship between can be expressed as follows:

[0149]

[0150] Where A∈R 6(N+1)×6 , B s ∈R 6(N+1)×6N , S e k ∈R 6×6 , A means and The state matrix of the relationship between s express and The control matrix of the relationship between e k Represents the current control step visual feature error S e The status value of .

[0151] The state space equation of the static visual predictive control system is transformed into the standard form of solving the optimization problem through the quadratic programming method:

[0152]

[0153] in, represents the upper bound matrix of the inequality constraints of the static visual predictive control system, represents the lower bound matrix of the inequality constraints of the static visual predictive control system, D s The coefficient matrix A represents the inequality constraints of the static visual predictive control system. s The coefficient matrix representing the equality constraints of the static visual predictive control system, b s Represents the equality constraint matrix of the static vision predictive control system.

[0154] S10: The standard form of the above optimization problem is obtained Figure 1 The solvable expression of SVPC shown in , the visual feature error S e Input the standard form of the optimization problem above to predict the input variables of the static visual predictive control system The prediction input variables of the static visual predictive control system The first element is the camera velocity v C , combined with the transformation matrix from the robot end coordinate system to the camera coordinate system Calculate the end velocity v of the robot arm e , according to the end velocity v of the robot arm e , combined with the inverse matrix J of the Jacobian matrix of the manipulator -1 Calculating the joint velocity of the robotic arm According to the obtained joint velocity Control the movement of the robotic arm and approach the static target to complete docking.

[0155] like Figure 2 As shown, it shows the actual situation of the robot arm completing the static docking task by applying the method of the embodiment of the present invention.

[0156] Although the embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art may make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

[0157] The above specific embodiments of the present invention do not constitute a limitation on the scope of protection of the present invention. Any other corresponding changes and modifications made based on the technical concept of the present invention should be included in the scope of protection of the claims of the present invention.

Claims

1. A robotic arm control method for automatic docking of static targets, characterized in that: include: S1: Set up a camera on the robotic arm and a force sensor on the end effector of the robotic arm; Setting a plurality of feature points, wherein the plurality of feature points are sequentially connected to form a target, and the position of the target is fixed relative to the static object; S2: docking the end effector of the manipulator with the static target, capturing the target image of the target through the camera, and obtaining the desired image moment feature of the target in the target image ; S3: During the docking process between the end effector of the manipulator and the static target, the camera collects the target image in real time, and obtains the real-time image moment feature of the target through the real-time target image. ; S4: Sense the actual external force on the end effector of the robotic arm through the force sensor , the actual external force in the Cartesian coordinate system and expected external forces The error is converted into the unit inertia virtual force error in the image feature space : ； in, represents the desired inertia matrix of the camera, Represents the transformation matrix from the camera coordinate system to the robot end coordinate system, Represents the real-time image moment feature The corresponding image Jacobian matrix; S5: Establish the state space equation of the force prediction control system: ； in, represents the state variable of the force predictive control system at the current control step, represents the state variable of the force predictive control system for the next control step, represents the control input variable of the force predictive control system, represents the control output variable of the force predictive control system, represents the sampling period, , represents the positive definite gain matrix, represents the unit inertial stiffness matrix; S6: The state space equation of the force prediction control system is transformed into a standard form for solving the optimization problem through the quadratic programming method: ； in, represents the loss function of the force predictive control system, represents the predicted input variable of the force predictive control system at the current control step, Indicates the current control step size The status value of represents the upper limit matrix of the inequality constraints of the force predictive control system, represents the lower bound matrix of the inequality constraints of the force predictive control system, , , , , The extended state vector representing the force predictive control system and The control matrix of the relationship between represents the weight matrix of the error term of the force predictive control system, represents the weight matrix of the input items of the force predictive control system, express and The state matrix of the relationship between represents the weight matrix of the terminal error term of the force predictive control system, The coefficient matrix representing the inequality constraints of the force predictive control system, The coefficient matrix representing the equality constraints of the force predictive control system, Represents the equality constraint matrix of the force predictive control system; S7: The unit inertia virtual force error Enter the standard form in S6 to obtain the predicted input variables of the force predictive control system , the predicted input variables The first element is used as the real-time image moment feature The amount of change , the change Feedback to the dynamic visual prediction control system to achieve cascade and reference image moment features : ； Calculating visual feature error : ； S8: Establish the state space equation of static visual predictive control system: ； in, Represents the state variable of the static visual predictive control system at the current control step, represents the state variable of the static visual predictive control system for the next control step, represents the control input variable of the static visual predictive control system, represents the control output variable of the static visual predictive control system, Represents the desired image moment feature The corresponding image Jacobian matrix; S9: The state space equations of the static visual predictive control system are transformed into a standard form for solving the optimization problem through the quadratic programming method: ； in, represents the loss function of the static visual predictive control system, represents the upper bound matrix of the inequality constraints of the static visual predictive control system, represents the lower bound matrix of the inequality constraints of the static visual predictive control system, represents the predicted input variable of the static visual predictive control system, Indicates the visual feature error of the current control step The status value of , , , , express and The control matrix of the relationship between represents the state error predictor variable, represents the weight matrix of the error term of the static visual predictive control system, represents the weight matrix of the input items of the static visual predictive control system, represents the weight matrix of the terminal error term of the static visual predictive control system, express and The state matrix of the relationship between The coefficient matrix representing the inequality constraints of the static visual predictive control system, The coefficient matrix representing the equality constraints of the static vision predictive control system, represents the equality constraint matrix of the static visual predictive control system; S10: The visual feature error Enter the standard form in S9 to obtain the predicted input variables of the static visual predictive control system , the predicted input variables The first element is the camera speed , according to the camera speed Calculating the joint velocity of the robotic arm , control the robot arm according to the joint speed Do some exercise.

2. The robotic arm control method for automatic docking of static targets according to claim 1, characterized in that: According to the target image, the expected image moment feature is calculated and real-time image moment features The expressions are: ； The expected image moment features Real-time image moment features The calculation method is: ； ； ； ； ； in, represents the expected area of ​​the target in the target image, represents the actual area of ​​the target in the target image, Indicates the depth between the camera and the target, represents the continuous origin moment of the target image, The image plane representing the target image, Indicates that the coordinates in the target image are The grayscale value of the pixel, represents the continuous center distance of the target image, Indicates the coordinates of the center of gravity of the target image.

3. The robotic arm control method for automatic docking of static targets according to claim 2, characterized in that: The image feature space is the image moment feature The corresponding space.

4. The robotic arm control method for automatic docking of static targets according to claim 1, characterized in that: The unit inertia virtual force error The solution is: A robotic arm model without inertia is established and converted into the image feature space as follows: ； in, ； Represents the Jacobian matrix of the image feature space, represents the inertia matrix of the robotic arm, represents the Coriolis force matrix, represents the gravity term, represents the torque output term of the robotic arm joint, Indicates the torque term that maps the actual external force to the robotic arm joint, It represents the unit inertia virtual force projected into the image feature space by the actual external force. represents the unit inertia virtual force, , express The first derivative of ; The desired inertia matrix of the camera Calculate the unit inertia virtual force : 。 5. The robotic arm control method for automatic docking of static targets according to claim 1, characterized in that: The state space equation of the force prediction control system is established by introducing a positive definite gain matrix into the visual admittance control in the image feature space. Establish the state space equation of the force predictive control system.

6. The robotic arm control method for automatic docking of static targets according to claim 1, characterized in that: The loss function of the force prediction control system is: ； in, Indicates the current control step prediction The unit inertia virtual force error corresponding to the control step size, Indicates the current control step prediction The unit inertia virtual force error corresponding to the control step size, Indicates the current control step prediction The control step size corresponds to the predicted input variable of the force prediction control system.

7. The robotic arm control method for automatic docking of static targets according to claim 1, characterized in that: The loss function of the static visual prediction control system is: ； in, represents the weight matrix of the error term of the static visual predictive control system, represents the weight matrix of the input items of the static visual predictive control system, represents the weight matrix of the terminal error term of the static visual predictive control system, Indicates the current control step prediction Control the visual feature error corresponding to the step size, Indicates the current control step prediction Control the visual feature error corresponding to the step size, Indicates the current control step prediction The predicted input variables of the static visual predictive control system corresponding to the control step size.

8. The robotic arm control method for automatic docking of static targets according to claim 1, characterized in that: The robot arm joint speed The calculation method is: According to the camera speed , through the transformation matrix from the robot end coordinate system to the camera coordinate system Calculate the end velocity of the robot arm , according to the end speed of the robot arm , through the inverse matrix of the Jacobian matrix of the manipulator Calculating the joint velocity of the robotic arm .

Images