A mechanical arm control method for dynamic target automatic docking

Abstract

The present application relates to the technical field of mechanical arm target docking, and specifically provides a mechanical arm control method for dynamic target automatic docking, which cascades a force prediction control system and a dynamic visual prediction control system, the force prediction control system outputs a variation of moment features of real-time images according to the force sensor on the mechanical arm to perceive the actual external force on the mechanical arm end, obtains target images at different time through the camera arranged on the mechanical arm end, and calculates visual feature errors in combination with the variation of moment features of real-time images, inputs the visual feature errors into the dynamic 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 comprehensively integrates the force sense and visual information, more comprehensively the position and state of the dynamic target, overcomes the limitations of the single sensor method, and improves the performance of the mechanical arm in completing the docking task.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm target docking, and specifically provides a mechanical arm control method for dynamic target automatic docking. BACKGROUND

[0002] The mechanical arm end automatic docking technology under dynamic target refers to the technology that enables the mechanical arm end to automatically and accurately dock to the target object in the case that the target object or target position is in motion or changes. This technology has important application prospects in the fields of automatic assembly, logistics processing, unmanned aerial vehicle automatic charging, etc. The key challenges of the mechanical arm end automatic docking technology under dynamic target include accurate perception and tracking of dynamic target, real-time adjustment and coordinated control of mechanical arm motion, force control in the docking process, etc. Therefore, this technology needs to comprehensively utilize technical means in the fields of visual perception, motion planning, control algorithm, etc. to realize the automatic docking of dynamic target. The methods currently used to realize the mechanical arm end automatic docking task under dynamic target mainly include the following:

[0003] Visual servoing control: the position and attitude of the target object are captured in real time by a visual sensor, and then a visual servoing control algorithm is used to make the mechanical arm end track the motion trajectory of the target object to realize the docking task.

[0004] Dynamic motion planning: the motion prediction information of the target object is utilized, combined with the dynamics model and motion planning algorithm of the mechanical arm, to generate the mechanical arm end trajectory that adapts to the motion of the target object in real time, so as to realize the docking task.

[0005] Force control docking: through a force sensor or a force control algorithm, the mechanical arm end applies appropriate force to the target object, thereby realizing the automatic docking task under dynamic target.

[0006] Machine learning method: machine learning technology is used to learn and train a large amount of dynamic docking task data, so that the mechanical arm end can adaptively dock different target objects under dynamic conditions.

[0007] The current methods have the following shortcomings when realizing the mechanical arm end automatic docking task under dynamic target:

[0008] 1. Limitations of visual perception: the visual sensor is sensitive to light, occlusion and other conditions, and may be disturbed by the environment, leading to inaccurate perception of the target object and affecting the stability of the docking task.

[0009] 2. Complexity of motion planning: the motion trajectory of the dynamic target is usually uncertain and complex, and the mechanical arm end trajectory that adapts to the target motion needs to be generated in real time, which puts higher requirements on the motion planning algorithm.

[0010] 3. Precision of force control: During the docking process, appropriate force needs to be applied to the end of the robot arm, but the accuracy and real-time performance of the force sensor may affect the accuracy of the docking task.

[0011] 4. High cost of learning and training: Using machine learning methods to achieve dynamic docking tasks requires a large amount of data for learning and training, which is costly and has high real-time requirements.

[0012] These shortcomings can limit the stability, accuracy and applicability of dynamic target automatic docking tasks. Therefore, further research and improvement of these methods are still needed to overcome their shortcomings and adapt to more extensive application scenarios.

[0013] By comprehensively utilizing visual and force sensing information, the position and state of the target object can be more comprehensively perceived, and the motion of the robot arm end and the applied force can be more accurately controlled, thereby improving the efficiency and reliability of dynamic target automatic docking tasks. Therefore, we propose a robot arm control method for dynamic target automatic docking, which combines visual and force sensing information to overcome the limitations of single sensor methods and improve the performance of the docking task. SUMMARY

[0014] To solve the above problems, the present application provides a robot arm control method for dynamic target automatic docking, which cascades a force prediction control system (FPC) and a dynamic visual prediction control (DVPC) in the corresponding control system. The FPC in the control system can improve the interaction ability of the robot arm with the external environment, and the DVPC can improve the perception ability of the robot arm to the environment. The cascade of the two can overcome the limitations of single sensor methods, improve the flexibility, adaptability, control performance and execution efficiency of the robot arm, and further improve the effectiveness, reliability and accuracy of the robot arm automatic docking task.

[0015] The robot arm control method for dynamic target automatic docking provided by the present application specifically includes:

[0016] S1: A camera is arranged on the robot arm, and a force sensor is arranged on the end effector of the robot arm;

[0017] A plurality of feature points are arranged, and the plurality of feature points are sequentially connected to form a target, the position of the target being fixed relative to the dynamic target;

[0018] S2: The end effector of the robot arm is docked with the dynamic target, the camera acquires a target image of the target, and the expected image moment feature S of the target in the target image is obtained d ;

[0019] S3: During the docking process of the end effector of the robot arm and the dynamic target, the camera acquires real-time target images in real time, and the real-time image moment feature S of the target is obtained through the real-time target images;

[0020] 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 :

[0021]

[0022] 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;

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

[0024]

[0025] 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;

[0026] 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:

[0027]

[0028] Among them, J c 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 F s e The status value of represents the upper limit matrix of the inequality constraints of the force predictive control system, Lower bound matrix representing inequality constraints of force predictive control system,

[0029] B f Control matrix Q representing relationship between and f Weight matrix R representing error term of force predictive control system f Weight matrix A representing input term of force predictive control system and State matrix T representing relationship between f Weight matrix D representing terminal error term of force predictive control system f Coefficient matrix A representing inequality constraints of force predictive control system f Coefficient matrix b representing equality constraints of force predictive control system f Equality constraints matrix of force predictive control system

[0030] S7: Calculate unit inertia virtual force error F s e Input the standard form in S6 to obtain the prediction input variable of force predictive control system Take the first element of the prediction input variable as the change of 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 = S - S r ;

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

[0035]

[0036] Wherein, S(k) represents the state variable of dynamic visual predictive control system at current control step, S(k+1) represents the state variable of dynamic visual predictive control system at next control step, represent the expected image moment feature S d corresponding image Jacobian matrix, v c ​(k) represents the camera velocity of the current control step dynamic visual predictive control system output, v represents the variation of the real-time image moment feature S caused by the dynamic target motion, V c (k) represents the control input variable of the dynamic visual predictive control system, Y s (k) represents the control output variable of the dynamic visual predictive control system;

[0037] S9: The state space equation of the dynamic visual predictive control system is converted into a standard form for solving the optimization problem by the quadratic programming method:

[0038]

[0039] wherein, represents the prediction input variable of the dynamic visual predictive control system, J c represents the loss function of the dynamic visual predictive control system, S e k represents the state value of the current control step visual feature error S e , H s represents the quadratic term coefficient matrix, E s represents the linear term coefficient matrix, represents the lower limit matrix of the inequality constraint of the dynamic visual predictive control system, represents the upper limit matrix of the inequality constraint of the dynamic visual predictive control system, D s represents the coefficient matrix of the inequality constraint of the dynamic visual predictive control system, A s represents the coefficient matrix of the equality constraint of the dynamic visual predictive control system, b s represents the equality constraint matrix of the dynamic visual predictive control system;

[0040] S10: The visual feature error S e is input into the standard form in S9 to obtain the prediction input variable J of the dynamic visual predictive control system. The first element V c1 (k) of the prediction input variable J c (k) is calculated according to the camera velocity v c (k) to calculate the joint velocity of the robot arm The robot arm is controlled to move according to the joint velocity .

[0041] Preferably, the expression of the expected image moment feature S d and the real-time image moment feature S is as follows:

[0042] [x n , y n , an , θ, γ, α] T ;

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

[0044]

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

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

[0047] wherein a * represents the desired area of the target in the target image, a represents the actual area of the target in the target image, Z * represents 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 gray value of the pixel with coordinates (x, y) in the target image, μ ij represents the continuous centroid 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 space [x n , y n , a n , θ, γ, α] T .

[0049] Preferably, the solving method of the unit inertia virtual force error F s e is as follows:

[0050] The inertia term-free robot arm model is established and is converted to the image feature space and is expressed as:

[0051]

[0052] wherein,

[0053]

[0054] J s J(q) represents Jacobian matrix of image feature space, M(q) represents inertia matrix of robot arm, C(q) represents Coriolis force matrix, g(q) represents gravity term, τ represents torque output term of robot arm joint, J T f ext f represents actual external force mapping to torque term of robot arm joint, F represents actual external force projecting to unit inertia virtual force under image feature space, f s F represents unit inertia virtual force, represents first derivative of J;

[0055] in desired inertia matrix M c * of camera, unit inertia virtual force F s is calculated.

[0056]

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

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

[0059]

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

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

[0062]

[0063] wherein, S e (k+N|k) represents visual feature error corresponding to k+N control step predicted by 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 prediction, V C (k+i|k) represents the predicted input variable of the dynamic visual predictive control system corresponding to the k+i control step predicted by the current control step prediction.

[0064] Preferably, the camera speed v c The calculation formula of (k) is:

[0065]

[0066] Preferably, the mechanical arm joint speed The calculation method is:

[0067] According to the camera speed v c (k), the transformation matrix from the mechanical arm end coordinate system to the camera coordinate system The mechanical arm end speed v e According to the mechanical arm end speed v e The inverse matrix J -1 The mechanical arm joint speed

[0068] Compared with the prior art, the present application can achieve the following beneficial effects:

[0069] Comprehensive utilization of visual and force information can more comprehensively perceive the position and state of the target object, and more accurately control the motion of the mechanical arm end and the applied force, thereby improving the efficiency and reliability of the dynamic target automatic docking task. Therefore, we propose a mechanical arm control method for dynamic target automatic docking, which combines visual and force information, can overcome the limitations of single sensor method, and improve the high-precision control and perception ability of the mechanical arm in executing dynamic docking task, improve the effectiveness, adaptability, accuracy and stability of the dynamic target automatic docking process. At the same time, the control method of the present application uses extended Kalman filter to estimate the motion speed of the dynamic target, realizes the compensation of the camera speed control quantity output by the dynamic visual predictive control system, thereby improving the dynamic docking ability of the mechanical arm to the dynamic target, and effectively realizes the high-precision dynamic docking of the mechanical arm end. BRIEF DESCRIPTION OF DRAWINGS

[0070] Figure 1 is a cascade schematic diagram of the force predictive control system and the dynamic visual predictive control system provided according to the embodiment of the present application;

[0071] Figure 2 is a schematic diagram of the actual situation of the mechanical arm completing the dynamic target docking task according to the embodiment of the present application;

[0072] Figure 3 Figure 2 is another schematic diagram of a dynamic target docking task actually performed by a mechanical arm according to an embodiment of the present application. DETAILED DESCRIPTION

[0073] Hereinafter, embodiments of the present application will be described with reference to the accompanying drawings. In the following description, the same components are denoted by the same reference numerals. In the case of the same reference numerals, their names and functions are also the same. Therefore, detailed descriptions thereof will not be repeated.

[0074] In order to make the objectives, technical solutions, and advantages of the present application clearer, further detailed descriptions will be given below in combination with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and do not constitute a limitation on the present application.

[0075] As shown in Figure 1 Figure 1, an embodiment of the present application provides a mechanical arm control method for dynamic target automatic docking, a control system of the control method cascades two different model predictive controls (MPCs), i.e., a force predictive control system (FPC) and a dynamic visual predictive control system (DVPC), and specifically includes the following steps:

[0076] S1: A camera and a force sensor are arranged on the mechanical arm, the camera is relatively fixed with the mechanical arm and can be installed on an end effector of the mechanical arm to move with the mechanical arm. The force sensor is installed on the end effector of the mechanical arm to move with the mechanical arm, and the force sensor can sense actual external force conditions to which the end effector of the mechanical arm is subjected in real time.

[0077] A plurality of feature points are randomly arranged, and the feature points are sequentially connected to form a pattern, which is referred to as a target. The target needs to meet the following requirements: the target position is fixed and unchangeable relative to the dynamic target, usually a region of a plane where the dynamic target is located is selected as the target, the size and region of the target are irrelevant to the docking task type performed by the mechanical arm, and the target can be any complete, characteristic, and position-specific pattern region. Four feature points are arranged in the embodiment of the present application, the four feature points are sequentially connected to form a rectangle, and the rectangle is taken as the target. The distance between the rectangle and the dynamic target is known and relatively fixed.

[0078] In addition, a pattern can also be selected as the target first, and the target needs to meet the above requirements. After the target is determined, a plurality of points on the target are selected as the feature points.

[0079] During the docking process between the end effector of the mechanical arm and the dynamic target, the camera can capture the target to obtain a target image in real time.

[0080] S2: the mechanical arm end effector is docked with the dynamic target, the docking method can be manual traction of the mechanical arm to make the end effector dock with the dynamic target, or the dynamic target is disassembled from the installation position and docked with the mechanical arm end effector, in the docking state, the target image is collected by the camera, and the image moment feature of the target in the target image is calculated to obtain the expected image moment feature S d , the expected image moment feature S d is the image moment feature corresponding to the docking task completed by the mechanical arm.

[0081] S3: after obtaining the expected image moment feature S d , the mechanical arm is separated from the dynamic target again, in the process of docking the mechanical arm end effector with the dynamic target, the camera collects the target image of the target in real time, and the real-time image moment feature S of the target in the target image is calculated according to the real-time target image.

[0082] The expression of the expected image moment feature S d and the real-time image moment feature S is

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

[0084] Among them,

[0085]

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

[0087] μ ij =∫∫ O (x-x g ) i (y-y g ) j f(x, y) dxdy;

[0088] a * represents the expected area of the target in the target image, a represents the actual area of the target in the target image, Z * represents the depth between the camera and the target, m pq represents the continuous origin moment of the target image, p and q represent the power of x and y respectively, O represents the image plane of the target image, f(x, y) represents the gray 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.

[0089] Components x n , y n , and a n are used to control the translation of the camera, i.e., the linear velocity (v x , v y , v z ) of the camera relative to the coordinate axes of the camera coordinate system, and components θ, γ, and a are used to control the rotation of the camera, i.e., the angular velocity (w x , w y , w z ) of the camera relative to the coordinate axes of the camera coordinate system, the camera coordinate system being a preset coordinate system, which is preset by a user according to an actual experimental environment, a task type, and a mechanical arm itself, the origin of the camera coordinate system being the center of mass of the camera, and the translation and rotation of the camera can be converted into the translation and rotation of the end of the mechanical arm, thereby controlling the movement of the mechanical arm.

[0090] S4: Perform inertia normalization processing on the traditional mechanical arm model, remove the inertia term in the mechanical arm model, and convert the processed mechanical arm model to image moment features [x n , y n , a n , θ, γ, a] T Corresponding space, the embodiment of the application refers to the space as an image feature space, and obtains the expression of the mechanical arm model without inertia terms as follows:

[0091]

[0092] wherein,

[0093]

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

[0095] An expression of the robot arm model in the image feature space can be transformed as:

[0096]

[0097] wherein,

[0098]

[0099] M c represents a projection of an inertia matrix of the robot arm in a camera coordinate system, represents a projection of the actual external force in the camera coordinate system, represents a transformation matrix from a robot arm end coordinate system to the camera coordinate system.

[0100] M c * is defined as a desired inertia matrix of the camera, the unit inertial virtual force F c * is calculated under the desired inertia matrix M s of the camera:

[0101]

[0102] wherein, F s represents the unit inertial virtual force under M c *-1 .

[0103] In a Cartesian coordinate system, during a process of docking between an end effector of a robot arm and a dynamic target, an actual external force f ext experienced on the end effector of the robot arm has an error with a desired external force f ext d The actual external force f ext is obtained by a force sensor in real time sensing a situation of the actual external force experienced by the end effector of the robot arm; the desired external force f ext d is a preset value, a specific value of which is related to a docking task requirement performed by the robot arm, for example, if the robot arm needs to complete a device docking, the desired external force f ext ​d The optimal value is zero Newton; if the robot needs to complete is the casting body grinding docking, assuming that the grinding required external force is 80 Newton, the expected external force f ext d The optimal value is 80 Newton. The actual external force f ext The error between the expected external force f ext d Convert the error into the unit inertia virtual force error F s e :

[0104]

[0105] Where F s e =F s -F s d , F s d Indicates the unit inertia virtual force in the image feature space.

[0106] S5: Admittance control is a kind of existing force control system in the field of robot control, which can ensure that the movement of the robot is more gentle and safer. Admittance control adjusts the movement of the robot by measuring the actual external force information received by the robot, and the control method can also be applied in the image feature space. The existing admittance control is converted and expressed in the image feature space to obtain a visual admittance model:

[0107]

[0108] Where m s Indicates the unit inertia virtual mass, b s Indicates the unit inertia damping, k s Indicates the unit inertia stiffness matrix, S * Indicates the change amount of real-time image moment feature S, S * =S d -S r , S d Indicates the expected image moment feature of the feature point in the target image, S r Indicates the reference image moment feature of the feature point in the target image.

[0109] S * =S d -S r Is introduced into the visual admittance model to obtain the following formula:

[0110]

[0111] In order to ensure that the force control system can respond quickly, a positive definite 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:

[0112]

[0113] During the docking process between the end effector of the manipulator and the dynamic 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:

[0114]

[0115] Wherein, M=-H.

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

[0117]

[0118] 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.

[0119] 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 and the predicted input variable of the force predictive control system in the current control step has the following expression:

[0120]

[0121] wherein F s e (k+N|k) represents the unit inertia virtual force error F s e , because the k+N control step is the last control step, F s e (k+N|k) is also called the terminal unit inertia virtual force error predicted in the current control step, F s e (k|k) = F s e k represents the state value of the unit inertia virtual force error F s e in the current control step, S * (k|k) represents the state value of the predicted input variable of the force predictive control system corresponding to the current control step, S * (k+N-1|k) represents the predicted input variable of the force predictive control system corresponding to the k+N-1 control step predicted in the current control step, F s e (k+i|k) represents the unit inertia virtual force error F s e corresponding to the k+i control step predicted in the current control step, S * (k+i|k) represents the predicted input variable of the force predictive control system corresponding to the k+i control step predicted in the current control step.

[0122] 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 variable, and the expression of the loss function of the force predictive control system is as follows:

[0123]

[0124] wherein 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 represents the weight matrix of the terminal error term of the force predictive control system, Q f , R f and T f are all symmetric positive definite matrices.

[0125] 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:

[0126]

[0127] 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.

[0128] Further deduction yields:

[0129]

[0130] Among them, A represents and The state matrix of the relationship between f express and The control matrix of the relationship between them, A∈R 6(N+1)×6 , B f ∈R 6(N+1)×6N ,

[0131] 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:

[0132]

[0133] 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 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.

[0134] 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 eInput the standard form of the optimization problem for the force predictive control system to obtain the predicted input variables of the force predictive control system 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 dynamic visual prediction control system to achieve cascade.

[0135] 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 :

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

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

[0138] S e =SS r .

[0139] 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 dynamic visual predictive control system shown in , therefore, it is possible to cascade the force predictive control system with the dynamic visual predictive control system.

[0140] S8: After establishing the state space equation of the force prediction control system, it is also necessary to establish the state space equation of the dynamic vision prediction control system.

[0141] Since the docking target is in real-time dynamic motion, the real-time image moment feature S is also changing in real time. The rate of change of the real-time image moment feature S is The camera's velocity v in the camera coordinate system c and the change in the real-time image moment feature S caused by the dynamic target motion The following relationship exists:

[0142]

[0143] Among them, L s Represents the image Jacobian matrix corresponding to the real-time image moment feature S.

[0144] The amount of change of the real-time image moment feature S caused by the camera motion satisfies the following formula:

[0145]

[0146] The amount of change of the real-time image moment feature S caused by the dynamic target motion is applied to the extended Kalman filter The state equation and the measurement equation of are respectively:

[0147]

[0148] wherein, represents the state variable corresponding to the current control step, represents the measurement variable corresponding to the current control step, represents the state transition matrix, and H k represents the state measurement matrix, and ω k represents the process noise, and satisfies P(ω)~(0,Q), that is, ω obeys the Gaussian distribution with the mean value of 0 and the covariance matrix of Q, and v k represents the measurement noise, and satisfies P(v)~(0,R), that is, v obeys the Gaussian distribution with the mean value of 0 and the covariance matrix of R.

[0149] According to the foregoing formula, the expression of is:

[0150]

[0151] wherein, represents the speed of the camera at the current control step, represents the joint speed of the robot arm at the current control step.

[0152] The extended Kalman filter is divided into a prediction step and an update step, and according to the state equation and the measurement equation of, the equation of the Kalman filter prediction step is:

[0153]

[0154] wherein, represents the prior state variable, represents the prior covariance matrix, and P k-1 represents, and Q represents, the equation of the Kalman filter prediction step can be calculated to obtain the prior state variable and the prior covariance matrix.

[0155] The equation of the Kalman filter update step is:

[0156] ​​​

[0157] where K k denotes the gain of the extended Kalman filter, denotes the posterior state variable, P k denotes the posterior covariance matrix, I denotes the identity matrix, P k - denotes the prior covariance matrix.

[0158] The state space equation of the dynamic visual predictive control system under discrete time is established as follows:

[0159]

[0160] where, S(k) denotes the state variable of the dynamic visual predictive control system at the current control step, S(k+1) denotes the state variable of the dynamic visual predictive control system at the next control step, denotes the expected image moment feature S d the corresponding image Jacobian matrix, v c (k) denotes the camera speed output by the dynamic visual predictive control system at the current control step, denotes the change amount of the real-time image moment feature S caused by the dynamic target motion, V c (k) denotes the control input variable of the dynamic visual predictive control system, Y s (k) denotes the control output variable of the dynamic visual predictive control system.

[0161] S9: A general method of using constrained linear model predictive control is used to set the corresponding loss function, and the loss function of the dynamic visual predictive control system is:

[0162]

[0163] where S e (k+N|k) denotes the visual feature error corresponding to the k+N control step predicted at the current control step, S e (k+i|k) denotes the visual feature error corresponding to the k+i control step predicted at the current control step, V C (k+i|k) denotes the predicted input variable of the dynamic visual predictive control system corresponding to the k+i control step predicted at the current control step.

[0164] The state space equation of the dynamic visual predictive control system is converted into a standard form of solving an optimization problem by using a quadratic programming method:

[0165]

[0166] where, represents the predicted input variable of the dynamic visual predictive control system, J c represents the loss function of the dynamic visual predictive control system, S e k Represents the current control step visual feature error S e The state value, H s represents the quadratic coefficient matrix, E s represents the linear coefficient matrix, represents the lower bound matrix of the inequality constraints of the dynamic visual predictive control system, Denotes the upper limit matrix of the inequality constraints of the dynamic visual predictive control system, D s The coefficient matrix A represents the inequality constraints of the dynamic visual predictive control system. s The coefficient matrix representing the equality constraints of the dynamic visual predictive control system, b s Represents the equality constraint matrix of the dynamic visual predictive control system.

[0167] S10: The visual feature error S e Enter the standard form in S9 to obtain the predicted input variables of the dynamic visual predictive control system By predicting the input variables The first element V c1 (k) Calculate the camera speed v c (k):

[0168]

[0169] According to the camera speed v c (k) 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 According to the joint velocity obtained Control the movement of the robotic arm and approach the dynamic target to complete docking.

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

[0171] 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.

[0172] The above detailed description of the application is not intended to limit the scope of the application. Various other changes and modifications of the application can be made by those skilled in the art without departing from the scope of the application.

Claims

1. A robotic arm control method for automatic docking of dynamic 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 dynamic target; S2: docking the end effector of the manipulator with the dynamic 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 dynamic 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 calculate the reference image moment features : ； Calculating visual feature error : ； S8: Since the docking target is in real-time dynamic motion, the real-time image moment feature Also changes in real time, real-time image moment features Rate of change , the camera's velocity in the camera coordinate system and real-time image moment features caused by dynamic target motion The amount of change The following relationship exists: ； in, Representation and real-time image moment features The corresponding image Jacobian matrix. Real-time image moment features caused by camera motion The amount of change Satisfy the following formula: ； Applying Extended Kalman Filter to Real-time Image Moment Features Caused by Dynamic Target Motion The amount of change Make an estimate; establish the state space equation of the dynamic visual predictive control system: ； in, Represents the state variable of the current control step dynamic visual predictive control system, represents the state variable of the next control step dynamic visual predictive control system, Represents the expected image moment feature The corresponding image Jacobian matrix, represents the camera speed output by the current control step dynamic visual prediction control system, Represents the real-time image moment features caused by dynamic target motion The amount of change, represents the control input variable of the dynamic visual predictive control system, represents the control output variable of the dynamic visual predictive control system; S9: The state space equations of the dynamic visual predictive control system are transformed into a standard form for solving the optimization problem through the quadratic programming method: ； in, represents the predicted input variable of the dynamic visual predictive control system, represents the loss function of the dynamic visual predictive control system, Indicates the visual feature error of the current control step The status value of represents the quadratic coefficient matrix, represents the linear coefficient matrix, represents the lower bound matrix of the inequality constraints of the dynamic visual predictive control system, represents the upper limit matrix of the inequality constraints of the dynamic visual predictive control system, The coefficient matrix representing the inequality constraints of the dynamic visual predictive control system, The coefficient matrix representing the equality constraints of the dynamic visual predictive control system, represents the equality constraint matrix of the dynamic visual predictive control system; S10: The visual feature error Enter the standard form in S9 to obtain the predicted input variables of the dynamic visual predictive control system , through the predictor input variables The first element of Calculating 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 dynamic 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 dynamic targets according to claim 1, 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 dynamic 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, Representing real-time image moment features The rate of change, ； 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 dynamic targets according to claim 1, characterized in that: The state space equation of the force prediction control system is established by introducing a positive gain matrix into the visual admittance control of 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 dynamic 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 dynamic targets according to claim 1, characterized in that: The loss function of the dynamic vision prediction control system is: ； in, 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 control step size corresponds to the predicted input variable of the dynamic visual predictive control system.

8. The robotic arm control method for automatic docking of dynamic targets according to claim 1, characterized in that: The camera speed The calculation formula is: ； in, Represents the predicted input variable The first element of .

9. The robotic arm control method for automatic docking of dynamic targets according to claim 1, wherein: 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