CN118700156A - A robotic arm control method for automatic docking of dynamic targets - Google Patents
A robotic arm control method for automatic docking of dynamic targets Download PDFInfo
- Publication number
- CN118700156A CN118700156A CN202410983876.6A CN202410983876A CN118700156A CN 118700156 A CN118700156 A CN 118700156A CN 202410983876 A CN202410983876 A CN 202410983876A CN 118700156 A CN118700156 A CN 118700156A
- Authority
- CN
- China
- Prior art keywords
- control system
- force
- target
- matrix
- dynamic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Program-controlled manipulators
- B25J9/16—Program controls
- B25J9/1694—Program controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明涉及机械臂目标对接技术领域，具体提供一种用于动态目标自动对接的机械臂控制方法，该控制方法将力预测控制系统与动态视觉预测控制系统级联，力预测控制系统依据机械臂上的力传感器感知机械臂末端受到实际外力的情况输出实时图像矩特征的变化量，通过机械臂末端设置的相机获不同时刻的靶标图像，并结合实时图像矩特征的变化量计算视觉特征误差，将视觉特征误差输入动态视觉预测控制系统获得预测输入变量的第一个元素，依据第一个元素计算机械臂的关节速度，进而实现机械臂运动控制。本发明控制方法综合了力觉和视觉信息，更全面的动态目标的位置和状态，克服了单一传感器方法的局限性，在完成对接任务中，提高了机械臂的性能。
The present invention relates to the field of robotic arm target docking technology, and specifically provides a robotic arm control method for automatic docking of dynamic targets. The control method cascades a force prediction control system and a dynamic visual prediction control system. The force prediction control system senses the actual external force applied to the end of the robotic arm based on the force sensor on the robotic arm and outputs the variation of the real-time image moment feature. The camera set at the end of the robotic arm obtains target images at different times, and calculates the visual feature error in combination with the variation of the real-time image moment feature. The visual feature error is input into the dynamic visual prediction control system to obtain the first element of the predicted input variable, and the joint speed of the robotic arm is calculated based on the first element, thereby realizing the motion control of the robotic arm. The control method of the present invention integrates force perception and visual information, and more comprehensively understands the position and state of dynamic targets, overcomes the limitations of the single sensor method, and improves the performance of the robotic arm in completing the docking task.
Description
Technical Field
The invention relates to the technical field of mechanical arm target docking, and particularly provides a mechanical arm control method for automatic docking of a dynamic target.
Background
The automatic docking technology of the tail end of the mechanical arm under the dynamic target refers to a technology which enables the tail end of the mechanical arm to be automatically and accurately docked to a target object under the condition that the target object or the target position moves or changes. The technology has important application prospects in the fields of automatic assembly, logistics treatment, unmanned aerial vehicle automatic charging and the like. The key challenges of the automatic docking technology of the tail end of the mechanical arm under the dynamic target include accurate sensing and tracking of the dynamic target, real-time adjustment and coordination control of the motion of the mechanical arm, force control in the docking process and the like. Therefore, the technology needs to comprehensively utilize technical means in multiple fields such as visual perception, motion planning, control algorithm and the like so as to realize automatic butt joint of dynamic targets. The current method for realizing the automatic butt joint task of the tail end of the mechanical arm under the dynamic target mainly comprises the following steps:
visual servo control: the position and the gesture of the target object are captured in real time by utilizing a visual sensor, and then the tail end of the mechanical arm is enabled to track the motion trail of the target object through a visual servo control algorithm, so that the butt joint task is realized.
Dynamic motion planning: and generating a tail end track of the mechanical arm which is suitable for the movement of the target object in real time by utilizing the movement prediction information of the target object and combining a dynamic model and a movement planning algorithm of the mechanical arm so as to realize a butt joint task.
Force control docking: through a force sensor or a force control algorithm, the purpose that the tail end of the mechanical arm applies proper force to a target object is achieved, and therefore an automatic butt joint task under a dynamic target is achieved.
The machine learning method comprises the following steps: by means of a machine learning technology, the tail end of the mechanical arm can be enabled to dynamically dock different target objects in a self-adaptive mode through learning and training data of a large number of dynamic docking tasks.
When the current methods realize the automatic butt joint task of the tail end of the mechanical arm under the dynamic target, the following defects exist:
1. Limitations of visual perception: the visual sensor is sensitive to conditions such as illumination and shielding, and can be subjected to environmental interference, so that the perception of a target object is inaccurate, and the stability of a docking task is affected.
2. Complexity of motion planning: the motion trail of the dynamic target is usually uncertain and complex, and the tail end trail of the mechanical arm adapting to the motion of the target needs to be generated in real time, which puts higher requirements on a motion planning algorithm.
3. Force control accuracy: in the docking process, a proper force needs to be applied to the tail end of the mechanical arm, but the accuracy and instantaneity of the force sensor can influence the accuracy of the docking task.
4. The learning and training costs are high: the machine learning method is utilized to realize the dynamic butt joint task, a large amount of data is needed for learning and training, the cost is high, and the real-time requirement is high.
These drawbacks lead to certain limitations in the stability, accuracy and applicability of the dynamic target auto-docking task. Accordingly, there is still a need to further develop and improve these methods to overcome the drawbacks thereof and to accommodate a wider range of application scenarios.
The position and the state of the target object can be sensed more comprehensively by comprehensively utilizing the information of vision and force sense, and the movement and the applied force of the tail end of the mechanical arm can be controlled more accurately, so that the efficiency and the reliability of the automatic butt joint task of the dynamic target are improved. Therefore, we propose a mechanical arm control method for automatic butt joint of dynamic targets, which combines visual and force sense information, can overcome the limitation of a single sensor method and improve the performance of a butt joint task.
Disclosure of Invention
The invention provides a mechanical arm control method for automatic butt joint of a dynamic target, wherein a force prediction control system (FPC) and Dynamic Visual Prediction Control (DVPC) are cascaded in a control system corresponding to the control method, the FPC in the control system can improve the interaction capability of the mechanical arm and the external environment, DVPC can improve the perception capability of the mechanical arm to the environment, the limitation of a single sensor method can be overcome by the cascade connection of the two methods, the flexibility, the adaptability, the control performance and the execution efficiency of the mechanical arm are improved, and the effectiveness, the reliability and the accuracy of an automatic butt joint task of the mechanical arm are further improved.
The invention provides a mechanical arm control method for automatic butt joint of a dynamic target, which specifically comprises the following steps:
s1: a camera is arranged on the mechanical arm, and a force sensor is arranged on an end effector of the mechanical arm;
setting a plurality of characteristic points, wherein the characteristic points are sequentially connected to form a target, and the position of the target is fixed relative to a dynamic target;
S2: docking the mechanical arm end effector with a dynamic target, and acquiring a target image of the target through a camera to acquire expected image moment characteristics S d of the target in the target image;
S3: in the process of docking the mechanical arm end effector with a dynamic target, a camera acquires a target image in real time, and a real-time image moment characteristic S of the target is obtained through the real-time target image;
S4: the error between the actual external force F ext and the expected external force F ext d in the Cartesian coordinate system is converted into a unit inertial virtual force error F s e in the image feature space by sensing the actual external force F ext received by the mechanical arm end effector through a force sensor:
Where M c * represents the desired inertial matrix of the camera, The transformation matrix from the camera coordinate system to the mechanical arm terminal coordinate system is represented, and L s represents an image jacobian matrix corresponding to the real-time image moment feature S;
s5: establishing a state space equation of a force prediction control system:
Wherein F s e (k) represents a state variable of the current control step force prediction control system, F s e (k+1) represents a state variable of the next control step force prediction control system, S * (k) represents a control input variable of the force prediction control system, Y f (k) represents a control output variable of the force prediction control system, T represents a sampling period, m= -H, H represents a positive gain matrix, and k s represents a unit inertial stiffness matrix;
s6: converting a state space equation of the force prediction control system into a standard form for solving an optimization problem by a quadratic programming method:
Wherein J c denotes a loss function of the force predictive control system, A predicted input variable representing the current control step size of the force prediction control system,A state value representing the current control step F s e,An upper matrix representing the inequality constraints of the force predictive control system,A lower limit matrix representing the inequality constraints of the force predictive control system,
B f represents an extended state vector of the force predictive control systemAnd (3) withControl matrix of the relation between the control matrix, Q f represents weight matrix of error item of force prediction control system, R f represents weight matrix of input item of force prediction control system, A representsAnd (3) withA state matrix of the relation, T f represents a weight matrix of a terminal error term of the force prediction control system, D f represents a coefficient matrix of inequality constraint of the force prediction control system, A f represents a coefficient matrix of equality constraint of the force prediction control system, and b f represents an equality constraint matrix of the force prediction control system;
S7: the standard form of inputting the unit inertial virtual force error F s e into S6 obtains the predicted input variable of the force prediction control system Will predict input variablesAs the variation amount S * of the real-time image moment feature S, the reference image moment feature S r is calculated:
Sr＝Sd-S*；
Calculating a visual characteristic error S e:
Se＝S-Sr；
s8: establishing a state space equation of a dynamic visual prediction control system:
Wherein S (k) represents the state variable of the current control step dynamic visual predictive control system, S (k+1) represents the state variable of the next control step dynamic visual predictive control system, An image jacobian matrix corresponding to the desired image moment feature S d, v c (k) represents the camera speed output by the current control step dynamic visual prediction control system,Representing the variation of the real-time image moment characteristics S caused by the motion of the dynamic target, V c (k) represents the control input variable of the dynamic visual prediction control system, and Y s (k) represents the control output variable of the dynamic visual prediction control system;
S9: converting a state space equation of the dynamic visual prediction control system into a standard form for solving an optimization problem by a quadratic programming method:
wherein, Representing the predicted input variables of the dynamic visual prediction control system, J c representing the loss function of the dynamic visual prediction control system, S e k representing the state value of the current control step visual characteristic error S e, H s representing the quadratic coefficient matrix, E s representing the quadratic coefficient matrix,A lower matrix representing dynamic visual predictive control system inequality constraints,An upper limit matrix representing the inequality constraint of the dynamic visual prediction control system, D s represents a coefficient matrix of the inequality constraint of the dynamic visual prediction control system, A s represents a coefficient matrix of the equality constraint of the dynamic visual prediction control system, and b s represents an equality constraint matrix of the dynamic visual prediction control system;
S10: inputting the visual characteristic error S e into the standard form in S9 to obtain the predicted input variable of the dynamic visual prediction control system By predicting input variablesThe first element V c1 (k) of the model (a) calculates the camera speed V c (k), and calculates the joint speed of the mechanical arm according to the camera speed V c (k)Controlling the mechanical arm to control the joint speedAnd performing exercise.
Preferably, according to the target image, the expressions for calculating the desired image moment feature S d and the real-time image moment feature S are:
[xn,yn,an,θ,γ,α]T；
the calculation method of the expected image moment feature S d and the real-time image moment feature S comprises the following steps:
mpq＝∫∫Oxpyqf(x，y)dxdy；
μij＝∫∫O(x-xg)i(y-yg)jf(x，y)dxdy；
Wherein a * represents a desired area of a target in a target image, a represents an actual area of the target in the target image, Z * represents a depth between a camera and the target, m pq represents a continuous origin moment of the target image, O represents an image plane of the target image, f (x, y) represents a gray value of a pixel having coordinates (x, y) in the target image, μ ij represents a continuous center distance of the target image, and (x g＝m10/m00,yg＝m01/m00) represents coordinates of a center of gravity of the target image.
Preferably, the image feature space is a space corresponding to the image moment feature [ x n,yn,an,θ,γ,α]T ].
Preferably, the solving method of the unit inertial virtual force error F s e is as follows:
And (3) establishing a mechanical arm model without an inertia term, and converting the mechanical arm model into an image characteristic space expressed as:
wherein,
J s denotes the jacobian matrix of the image feature space, M (q) denotes the inertial matrix of the mechanical arm,Representing a Coriolis force matrix, g (q) representing a gravity term, τ representing a torque output term of the mechanical arm joint, J Tfext representing a torque term of the mechanical arm joint mapped to an actual external force,Representing the unit inertial virtual force of the projection of the actual external force to the image feature space, f s representing the unit inertial virtual force, Represents the first derivative of J;
The unit inertial virtual force F s is calculated under the expected inertial matrix M c * of the camera:
preferably, the method for establishing the state space equation of the force prediction control system comprises the following steps: the state space equation of the force prediction control system is established by introducing a positive gain matrix H in the visual admittance control of the image feature space.
Preferably, the loss function of the force predictive control system is:
Wherein, F s e (k+n|k) represents the unit inertial virtual force error corresponding to the k+n control step of the current control step prediction, F s e (k+i|k) represents the unit inertial virtual force error corresponding to the k+i control step of the current control step prediction, and S * (k+i|k) represents the prediction input variable of the force prediction control system corresponding to the k+i control step of the current control step prediction.
Preferably, the loss function of the dynamic visual predictive control system is:
Wherein S e (k+n|k) represents a visual characteristic error corresponding to the k+n control step of the current control step prediction, S e (k+i|k) represents a visual characteristic error corresponding to the k+i control step of the current control step prediction, and V C (k+i|k) represents a prediction input variable of the dynamic visual prediction control system corresponding to the k+i control step of the current control step prediction.
Preferably, the camera speed v c (k) is calculated as:
preferably, the speed of the mechanical arm joint The calculation method of (1) is as follows:
By transforming the matrix from the robot arm end coordinate system to the camera coordinate system according to the camera speed v c (k) Calculating the tail end speed v e of the mechanical arm, and calculating the joint speed of the mechanical arm according to the tail end speed v e of the mechanical arm through the inverse matrix J -1 of the jacobian matrix of the mechanical arm
Compared with the prior art, the invention has the following beneficial effects:
The position and the state of the target object can be sensed more comprehensively by comprehensively utilizing the information of vision and force sense, and the movement and the applied force of the tail end of the mechanical arm can be controlled more accurately, so that the efficiency and the reliability of the automatic butt joint task of the dynamic target are improved. Therefore, the control method for the mechanical arm for automatic butt joint of the dynamic target is provided, the limitation of a single sensor method can be overcome by combining visual and force sense information, the cooperative control of the movement of the mechanical arm and the contact force of the tail end is realized, the high-precision control and the perception capability of the mechanical arm when the dynamic butt joint task is executed are improved, and the effectiveness, the adaptability, the accuracy and the stability of the automatic butt joint process of the dynamic target are improved. Meanwhile, the control method of the invention uses the extended Kalman filter to estimate the motion speed of the dynamic target, and compensates the camera speed control quantity output by the dynamic vision prediction control system, thereby improving the dynamic butt joint capability of the mechanical arm on the dynamic target and effectively realizing the high-precision dynamic butt joint of the tail end of the mechanical arm.
Drawings
FIG. 1 is a schematic diagram of a cascade of a force predictive control system and a dynamic visual predictive control system provided in accordance with an embodiment of the invention;
Fig. 2 is a schematic diagram of an actual situation that a mechanical arm completes a dynamic target docking task according to an embodiment of the present invention;
Fig. 3 is another schematic diagram of an actual situation of the mechanical arm completing the dynamic target docking task according to the embodiment of the present invention.
Detailed Description
Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the following description, like modules are denoted by like reference numerals. In the case of the same reference numerals, their names and functions are also the same. Therefore, a detailed description thereof will not be repeated.
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be further described in detail with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not to be construed as limiting the invention.
As shown in fig. 1, an embodiment of the present invention provides a control method for a mechanical arm for automatic docking of a dynamic target, where a control system of the control method is cascaded with two different Model Predictive Control (MPC), namely, a force predictive control system (FPC) and a dynamic visual predictive control system (DVPC), and specifically includes the following steps:
S1: the camera and the force sensor are arranged on the mechanical arm, and the camera and the mechanical arm are relatively fixed and can be arranged on an end effector of the mechanical arm to move along with the mechanical arm. The force sensor is arranged on the mechanical arm end effector and moves along with the mechanical arm, and the force sensor can sense the actual external force condition of the mechanical arm end effector in real time.
And randomly setting a plurality of characteristic points, and sequentially connecting the characteristic points to form a pattern, wherein the pattern is called as a target. The target needs to meet the following requirements: the target position is fixed relative to the dynamic target, a region of the plane where the dynamic target is located is usually selected as the target, the factors such as the size and the region of the target are irrelevant to the type of the docking task executed by the mechanical arm, and the target can be any complete, characteristic and clear-positioned graphic region. In the embodiment of the invention, four characteristic points are arranged and are sequentially connected to form a rectangle, the rectangle is taken as a target, and the distance between the rectangle and a dynamic target is known and relatively fixed.
In addition, a pattern can be selected as a target, the target requirement is met, and a plurality of points on the target are selected as characteristic points after the target is determined.
In the process of docking the mechanical arm end effector with a dynamic target, the camera can shoot a target in real time to acquire a target image.
S2: the method for realizing the butt joint of the mechanical arm end effector and the dynamic target can be that the mechanical arm is manually pulled to enable the end effector to be in butt joint with the dynamic target, or the dynamic target is dismounted from the mounting position and then is in butt joint with the mechanical arm end effector, in the butt joint state, the target image is acquired through the camera, the image moment characteristics of the target in the target image in the state are calculated and obtained, namely the expected image moment characteristics S d, and the expected image moment characteristics S d are the corresponding image moment characteristics when the mechanical arm completes the butt joint task.
S3: after the expected image moment feature S d is obtained, the mechanical arm is separated from the dynamic target again, the camera acquires the target image of the target in real time in the process of docking the mechanical arm end effector with the dynamic target, and the real-time image moment feature S of the target in the target image is obtained through calculation according to the real-time target image.
The desired image moment feature S d and the real-time image moment feature S are both expressed as
[xn,yn,an,θ,γ,α]T；
Wherein,
mpq＝∫∫Oxpyqf(x，y)dxdy；
μij＝∫∫O(x-xg)i(y-yg)jf(x，y)dxdy；
A * denotes a desired area of a target in a target image, a denotes an actual area of the target in the target image, Z * denotes a depth between a camera and the target, m pq denotes a continuous origin moment of the target image, p and q denote powers of x and y, respectively, O denotes an image plane of the target image, f (x, y) denotes a gray value of a pixel having coordinates (x, y) in the target image, μ ij denotes a continuous center distance of the target image, and (x g＝m10/m00,yg＝m01/m00) denotes coordinates of a center of gravity of the target image.
The components x n、yn and a n are used for controlling translation of the camera, namely, linear speed (v x,vy,vz) of the camera relative to a coordinate axis of a camera coordinate system, the components θ, γ and α are used for controlling rotation of the camera, namely, angular speed (w x,wy,wz) of the camera relative to the coordinate axis of the camera coordinate system, the camera coordinate system is a preset coordinate system, the origin of the camera coordinate system is a camera centroid according to actual experimental environment, task type and self condition of the mechanical arm, and translation and rotation of the camera can be converted into translation and rotation of the tail end of the mechanical arm, so that movement of the mechanical arm is controlled.
S4: the method comprises the steps of carrying out inertia normalization processing on a traditional mechanical arm model, removing inertia items in the mechanical arm model, and converting the processed mechanical arm model into a space corresponding to image moment characteristics [ x n,yn,an,θ,γ,α]T ], wherein the space is called as an image characteristic space, and the following expression of the mechanical arm model without the inertia items is obtained:
wherein,
J s denotes the jacobian matrix in the image feature space, M (q) denotes the inertial matrix of the robot arm,Representing the coriolis force matrix,J represents the jacobian matrix of the robotic arm,The first derivative of J is represented as,Representing a transformation matrix from a mechanical arm end coordinate system to a camera coordinate system, wherein the mechanical arm end coordinate system is a preset coordinate system which is preset by a user according to an actual experimental environment, a task type and a mechanical arm self condition, the origin of the mechanical arm end coordinate system is the mass center of a mechanical arm end joint, L s represents an image jacobian matrix corresponding to a real-time image moment characteristic S, L s∈R6×6, g (q) represents a gravity item, tau represents a torque output item of the mechanical arm joint, f ext represents an actual external force acting on a mechanical arm end effector, J Tfext represents a torque item of f ext mapped to the mechanical arm joint,Representing the unit inertial virtual force of the projection of the actual external force to the image feature space, and f s represents the unit inertial virtual force;
in a mechanical arm model under an image feature space The expression of (2) can be transformed into:
wherein,
M c denotes the projection of the robot inertial matrix in the camera coordinate system,Representing the projection of the actual external force in the camera coordinate system,Representing a transformation matrix from the robot arm end coordinate system to the camera coordinate system.
Defining M c * as the expected inertial matrix of the camera, calculating the unit inertial virtual force F s under the expected inertial matrix M c * of the camera:
Wherein F s represents the unit inertial virtual force at M c *-1.
In the Cartesian coordinate system, in the process of butting the mechanical arm end effector with a dynamic target, an error exists between an actual external force f ext borne by the mechanical arm end effector and an expected external force f ext d, and a force sensor senses the actual external force borne by the mechanical arm end effector in real time to obtain an actual external force f ext; the expected external force f ext d is a preset value, and the specific value of the expected external force f ext d is related to the requirement of the docking task executed by the mechanical arm, for example, if the mechanical arm needs to complete the docking of the device, the expected external force f ext d is optimal to be zero newton; if the mechanical arm is required to finish the grinding and butt joint of the casting body, and the external force required by grinding is 80 newtons, the optimal value of the expected external force f ext d is 80 newtons. The error between the actual external force F ext and the expected external force F ext d is converted into a unit inertial virtual force error F s e in the image feature space:
Where F s e＝Fs-Fs d,Fs d represents the unit inertial virtual force in image feature space.
S5: admittance control is an existing force control system in the field of mechanical arm control, can ensure softer and safer movement of the mechanical arm, adjusts the movement of the mechanical arm by measuring actual external force information received by the mechanical arm, and can be applied to an image feature space. The existing admittance control is converted into an image feature space to be expressed, and a visual admittance model is obtained:
Wherein m s represents a unit inertial virtual mass, b s represents a unit inertial damping, k s represents a unit inertial stiffness matrix, S * represents a variation of a real-time image moment feature S, S *＝Sd-Sr,Sd represents a desired image moment feature of a feature point in a target image, and S r represents a reference image moment feature of the feature point in the target image.
S *＝Sd-Sr was introduced into the visual admittance model to obtain the following formula:
to ensure that the force control system can respond quickly, a positive gain matrix H is introduced to ensure that F s e decreases exponentially, after which the following expression is obtained:
In the process of docking the mechanical arm end effector with the dynamic target, when the value of the variation S * of the real-time image moment characteristic S is unchanged, the mechanical arm is said to be in a balanced state, and at the moment AndConverging to zero, willAndThe value of (2) is taken as 0 to simplify the visual admittance model, andIntroducing into simplified visual admittance model to obtain unit inertial virtual force error derivativeThe relation with the variation S * of the real-time image moment characteristic S is as follows:
Wherein m= -H.
Through the calculation, the state space equation of the force prediction control system (FPC) under discrete time is obtained as follows:
Wherein, F s e (k) represents a state variable of the current control step force prediction control system, F s e (k+1) represents a state variable of the next control step force prediction control system, S * (k) represents a control input variable of the force prediction control system, Y f (k) represents a control output variable of the force prediction control system, and T represents a sampling period, wherein the sampling period is a sampling period of the force sensor for collecting the actual external force and a period of extracting the feature point from the target image obtained by the camera, and the two periods are the same.
S6: the Model Predictive Control (MPC) includes a prediction horizon N P and a control horizon N c, where the prediction horizon N P is used to predict a time range of future system behavior and the control horizon N c is used to calculate a time range of optimal control inputs. Let N P＝Nc = N to simplify the control algorithm and reduce the control time domain, expand the state vectorPredicted input variable of sum force prediction control system at current control stepHas the following expression:
Wherein F s e (k+n|k) represents a unit inertial virtual force error F s e corresponding to a k+n control step of the current control step prediction, F s e (k+n|k) is also referred to as a terminal unit inertial virtual force error of the current control step prediction, F s e(k|k)＝Fs e k represents a state value of a unit inertial virtual force error F s e at the current control step, S * (k|k) represents a state value of a predicted input variable of the force prediction control system corresponding to the current control step, S * (k+n-1|k) represents a predicted input variable of the force prediction control system corresponding to a k+n-1 control step of the current control step prediction, F s e (k+i|k) represents a unit inertial virtual force error F s e,S* (k+i|k) corresponding to a k+i control step of the current control step prediction.
By using a general method of constrained linear model predictive control, a loss function of a force predictive control system is set to obtain an optimal control variable, and the loss function expression of the force predictive control system is as follows:
Wherein, Q f represents the weight matrix of the error term of the force prediction control system, R f represents the weight matrix of the input term of the force prediction control system, T f represents the weight matrix of the terminal error term of the force prediction control system, and Q f、Rf and T f are both symmetrical positive definite matrices.
The state values F s e (k|k) and S * (k+i-1-j|k) of the current control step F s e are expressed as follows, depending on the loss function of the force-predictive control system:
Wherein S * (k+i-1-j|k) represents a predicted input variable of the force prediction control system corresponding to the k+i-1-j control step predicted by the current control step.
Further deriving:
Wherein A represents And (3) withState matrix of the relationship between each other, B f representsAnd (3) withA control matrix of the relationship between A epsilon R 6(N+1)×6,Bf∈R6(N+1)×6N,
Converting the state space equation of the force prediction control system into a solution optimization problem to obtain an optimal control sequence, and converting the state space equation of the force prediction control system into a standard form for solving the optimization problem through a quadratic programming method:
wherein, An upper matrix representing the inequality constraints of the force predictive control system,A lower limit matrix representing the inequality constraint of the force prediction control system, D f a coefficient matrix representing the inequality constraint of the force prediction control system, a f a coefficient matrix representing the equality constraint of the force prediction control system, and b f a coefficient matrix representing the equality constraint of the force prediction control system.
S7: the standard form of the solution optimization problem is the solvable expression of the FPC shown in FIG. 1, and the unit inertial virtual force error F s e under the image feature space is input into the standard form of the solution optimization problem of the force prediction control system, so that the prediction input variable of the force prediction control system can be obtainedThe first element of the predicted input variable is used as the variation S * of the real-time image moment characteristic S, namely the final output of the force prediction control system, and the variation S * is fed back to the dynamic visual prediction control system to realize cascading.
The reference image moment feature S r is calculated from the amount of change S * of the real-time image moment feature S and the desired image moment feature S d:
Sr＝Sd-S*。
Calculating a visual feature error S e from the real-time image moment feature S and the reference image moment feature S r:
Se＝S-Sr。
The solution process of the visual characteristic error S e introduces the variation S * of the output value real-time image moment characteristic S of the force prediction control system, and in the subsequent control process, the visual characteristic error S e is the input value of the dynamic visual prediction control system shown in fig. 1, so that the force prediction control system and the dynamic visual prediction control system can be cascaded.
S8: after the state space equation of the force prediction control system is established, the state space equation of the dynamic visual prediction control system is also required to be established.
Because the butt-joint target dynamically moves in real time, the real-time image moment characteristics S also change in real time, and the change rate of the real-time image moment characteristics SCamera speed v c of camera in camera coordinate system and amount of change of real-time image moment feature S caused by dynamic object motionThe following relationship exists:
where L s represents the image jacobian matrix corresponding to the real-time image moment feature S.
Variation of real-time image moment features S caused by camera motionSatisfies the following formula:
application of extended kalman filter to variation of real-time image moment characteristics S caused by dynamic object motion Estimating to obtainThe state equation and the measurement equation of (2) are respectively:
wherein, Representing the state variable corresponding to the current control step,Representing the measured variable corresponding to the current control step,Representing a state transition matrix. H k = [1······0] representing the state measurement matrix, omega k is representative of the process noise, and satisfies P (omega) to P (0, Q), namely omega obeys Gaussian distribution with mean value of 0 and covariance matrix of Q, v k represents the measurement noise and satisfies P (v) to (0, R), i.e. v obeys a gaussian distribution with a mean of 0 and a covariance matrix of R.
According to the formula, can obtainThe expression of (2) is:
wherein, Representing the speed of the camera at the current control step,Indicating the joint speed of the manipulator at the current control step.
The extended Kalman filtering is divided into a prediction step and an updating step according to the followingThe equation of the Kalman filtering prediction step is obtained by the state equation and the measurement equation:
wherein, Representing the a priori state variables,The equation representing the prior covariance matrix, P k-1, Q representing the kalman filter prediction step may be calculated to obtain the prior state variable and the prior covariance matrix.
The equation for the Kalman filter update step is:
wherein K k denotes the gain of the extended Kalman filter, Representing posterior state variables, P k represents the posterior covariance matrix, I represents the identity matrix, and P k - represents the a priori covariance matrix.
Establishing a state space equation of a dynamic visual prediction control system under discrete time:
wherein, S (k) represents the state variable of the current control step dynamic visual prediction control system, S (k+1) represents the state variable of the next control step dynamic visual prediction control system,An image jacobian matrix corresponding to the desired image moment feature S d, v c (k) represents the camera speed output by the current control step dynamic visual prediction control system,Representing the amount of change in the real-time image moment characteristics S caused by the motion of the dynamic target, V c (k) represents the control input variable of the dynamic visual prediction control system, and Y s (k) represents the control output variable of the dynamic visual prediction control system.
S9: the general method of constrained linear model predictive control is utilized to set corresponding loss functions, and the loss functions of the dynamic visual predictive control system are as follows:
Wherein S e (k+n|k) represents a visual characteristic error corresponding to the k+n control step of the current control step prediction, S e (k+i|k) represents a visual characteristic error corresponding to the k+i control step of the current control step prediction, and V C (k+i|k) represents a prediction input variable of the dynamic visual prediction control system corresponding to the k+i control step of the current control step prediction.
Converting a state space equation of the dynamic visual prediction control system into a standard form for solving an optimization problem by a quadratic programming method:
wherein, Representing the predicted input variables of the dynamic visual prediction control system, J c representing the loss function of the dynamic visual prediction control system, S e k representing the state value of the current control step visual characteristic error S e, H s representing the quadratic coefficient matrix, E s representing the quadratic coefficient matrix,A lower matrix representing dynamic visual predictive control system inequality constraints,The upper limit matrix representing the inequality constraint of the dynamic visual prediction control system, D s represents the coefficient matrix of the inequality constraint of the dynamic visual prediction control system, A s represents the coefficient matrix of the equality constraint of the dynamic visual prediction control system, and b s represents the equality constraint matrix of the dynamic visual prediction control system.
S10: inputting the visual characteristic error S e into the standard form in S9 to obtain the predicted input variable of the dynamic visual prediction control systemBy predicting input variablesThe first element V c1 (k) of (a) calculates the camera speed V c (k):
by transforming the matrix from the robot arm end coordinate system to the camera coordinate system in dependence of the camera speed v c (k) Calculating the tail end speed v e of the mechanical arm, and calculating the joint speed of the mechanical arm according to the tail end speed v e of the mechanical arm through the inverse matrix J -1 of the jacobian matrix of the mechanical armAccording to the obtained joint velocityAnd controlling the mechanical arm to move and approach to the dynamic target to finish the butt joint.
As shown in fig. 2 and 3, the practical situation that the mechanical arm completes the dynamic docking task is shown by applying the method of the embodiment of the invention.
While embodiments of the present invention have been illustrated and described above, it will be appreciated that the above described embodiments are illustrative and should not be construed as limiting the invention. Variations, modifications, alternatives and variations of the above-described embodiments may be made by those of ordinary skill in the art within the scope of the present invention.
The above embodiments of the present invention do not limit the scope of the present invention. Any other corresponding changes and modifications made in accordance with the technical idea of the present invention shall be included in the scope of the claims of the present invention.
Claims (9)
1. The mechanical arm control method for automatic docking of the dynamic target is characterized by comprising the following steps of:
s1: a camera is arranged on the mechanical arm, and a force sensor is arranged on an end effector of the mechanical arm;
setting a plurality of characteristic points, wherein the characteristic points are sequentially connected to form a target, and the position of the target is fixed relative to a dynamic target;
s2: docking the mechanical arm end effector with a dynamic target, acquiring a target image of the target through the camera, and acquiring expected image moment characteristics S d of the target in the target image;
S3: in the process of docking the mechanical arm end effector with a dynamic target, the camera acquires the target image in real time, and the real-time image moment characteristic S of the target is obtained through the real-time target image;
S4: the force sensor senses the actual external force f ext applied to the mechanical arm end effector, and the error between the actual external force f ext and the expected external force f ext d under the Cartesian coordinate system is converted into a unit inertial virtual force error under the image characteristic space
Wherein, Representing a desired inertial matrix of the camera,Representing a transformation matrix from a camera coordinate system to a mechanical arm terminal coordinate system, wherein L s represents an image jacobian matrix corresponding to the real-time image moment feature S;
s5: establishing a state space equation of a force prediction control system:
wherein, A state variable representing the current control step force predictive control system,Representing a state variable of a next control step force prediction control system, S * (k) representing a control input variable of the force prediction control system, Y f (k) representing a control output variable of the force prediction control system, T representing a sampling period, m= -H, H representing a positive gain matrix, and k s representing a unit inertial stiffness matrix;
s6: converting a state space equation of the force prediction control system into a standard form for solving an optimization problem by a quadratic programming method:
Where J c denotes the loss function of the force predictive control system, S *′k denotes the predicted input variable of the force predictive control system at the current control step, A state value representing the current control step F s e,An upper matrix representing the inequality constraints of the force predictive control system,A lower limit matrix representing the inequality constraints of the force predictive control system, B f represents an extended state vector of the force predictive control systemControl matrix related to S *′k, Q f represents weight matrix of error term of force prediction control system, R f represents weight matrix of input term of force prediction control system, and A representsA state matrix related to S *′k, T f represents a weight matrix of a terminal error term of the force prediction control system, D f represents a coefficient matrix of inequality constraint of the force prediction control system, A f represents a coefficient matrix of equality constraint of the force prediction control system, and b f represents a equality constraint matrix of the force prediction control system;
S7: inputting the unit inertial virtual force error F s e into a standard form in S6 to obtain a predicted input variable S *′k of a force prediction control system, taking a first element of the predicted input variable S *′k as a variation S * of a real-time image moment characteristic S, and calculating a reference image moment characteristic S r:
Sr＝Sd-S*；
Calculating a visual characteristic error S e:
Se＝S-Sr；
s8: establishing a state space equation of a dynamic visual prediction control system:
Wherein S (k) represents the state variable of the current control step dynamic visual predictive control system, S (k+1) represents the state variable of the next control step dynamic visual predictive control system, An image jacobian matrix corresponding to the desired image moment feature S d, v c (k) represents the camera speed output by the current control step dynamic visual prediction control system,Representing the variation of the real-time image moment characteristics S caused by the motion of the dynamic target, V c (k) represents the control input variable of the dynamic visual prediction control system, and Y s (k) represents the control output variable of the dynamic visual prediction control system;
S9: converting a state space equation of the dynamic visual prediction control system into a standard form for solving an optimization problem by a quadratic programming method:
wherein, Representing the predicted input variables of the dynamic visual prediction control system, J c representing the loss function of the dynamic visual prediction control system, S e k representing the state value of the current control step visual characteristic error S e, H s representing the quadratic coefficient matrix, E s representing the quadratic coefficient matrix,A lower matrix representing dynamic visual predictive control system inequality constraints,An upper limit matrix representing the inequality constraint of the dynamic visual prediction control system, D s represents a coefficient matrix of the inequality constraint of the dynamic visual prediction control system, A s represents a coefficient matrix of the equality constraint of the dynamic visual prediction control system, and b s represents an equality constraint matrix of the dynamic visual prediction control system;
s10: inputting the visual characteristic error S e into the standard form in S9 to obtain the predicted input variable of the dynamic visual prediction control system By predicting the input variablesThe first element V c( k) of (a) calculates a camera speed V c (k), and calculates a manipulator joint speed from the camera speed V c (k)Controlling the mechanical arm to control the joint speedAnd performing exercise.
2. The method for controlling a robotic arm for automatic docking of a dynamic target according to claim 1, wherein the expressions for calculating the desired image moment feature S d and the real-time image moment feature S from the target image are:
[xn,yn,an,θ,γ,α]T；
the calculation method of the expected image moment feature S d and the real-time image moment feature S comprises the following steps:
mpq＝∫∫Oxpyqf(x，y)dxdy；
μij＝∫∫o(x-xg)i(y-yg)jf(x，y)dxdy；
Wherein a * represents a desired area of a target in a target image, a represents an actual area of the target in the target image, Z * represents a depth between a camera and the target, m pq represents a continuous origin moment of the target image, O represents an image plane of the target image, f (x, y) represents a gray value of a pixel having coordinates (x, y) in the target image, μ ij represents a continuous center distance of the target image, and (x g＝m10/m00,yg＝m01/m00) represents coordinates of a center of gravity of the target image.
3. The method for controlling a robotic arm for automatic docking of a dynamic target according to claim 1, wherein the image feature space is a space corresponding to an image moment feature [ x n,yn,an,θ,γ,α]T ].
4. The method for controlling a mechanical arm for automatic docking of a dynamic target according to claim 1, wherein the solving method of the unit inertial virtual force error F s e is as follows:
And (3) establishing a mechanical arm model without an inertia term, and converting the mechanical arm model into an image characteristic space expressed as:
wherein,
J s denotes the jacobian matrix of the image feature space, M (q) denotes the inertial matrix of the mechanical arm,Representing a Coriolis force matrix, g (q) representing a gravity term, τ representing a torque output term of the mechanical arm joint, J Tfext representing a torque term of the mechanical arm joint mapped to an actual external force,Representing the unit inertial virtual force of the projection of the actual external force to the image feature space, f s representing the unit inertial virtual force,J represents the first derivative of J;
Calculating a unit inertial virtual force F s under a desired inertial matrix M c * of the camera:
5. The method for controlling a mechanical arm for automatic docking of a dynamic target according to claim 1, wherein the method for establishing a state space equation of the force prediction control system is as follows: the state space equation of the force prediction control system is established by introducing a positive gain matrix H in the visual admittance control of the image feature space.
6. The robotic arm control method for automatic dynamic target docking of claim 1, wherein the loss function of the force predictive control system is:
Wherein, F s e (k+n|k) represents the unit inertial virtual force error corresponding to the k+n control step of the current control step prediction, F s e (k+i|k) represents the unit inertial virtual force error corresponding to the k+i control step of the current control step prediction, and S * (k+i|k) represents the prediction input variable of the force prediction control system corresponding to the k+i control step of the current control step prediction.
7. The robotic arm control method for automatic docking of a dynamic target of claim 1, wherein the loss function of the dynamic visual predictive control system is:
Wherein S e (k+n|k) represents a visual characteristic error corresponding to the k+n control step of the current control step prediction, S e (k+i|k) represents a visual characteristic error corresponding to the k+i control step of the current control step prediction, and V C (k+i|k) represents a prediction input variable of the dynamic visual prediction control system corresponding to the k+i control step of the current control step prediction.
8. The robot arm control method for automatic docking of a dynamic object according to claim 1, wherein the camera speed v c (k) is calculated as:
9. The method for controlling a robot arm for automatic docking of a dynamic object according to claim 1, wherein the robot arm joint speed The calculation method of (1) is as follows:
by transforming the matrix from the robot arm end coordinate system to the camera coordinate system according to the camera speed v c (k) Calculating the tail end speed v e of the mechanical arm, and calculating the joint speed of the mechanical arm according to the tail end speed v e of the mechanical arm through an inverse matrix J -1 of a jacobian matrix of the mechanical arm
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410983876.6A CN118700156B (en) | 2024-07-22 | 2024-07-22 | A robotic arm control method for automatic docking of dynamic targets |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410983876.6A CN118700156B (en) | 2024-07-22 | 2024-07-22 | A robotic arm control method for automatic docking of dynamic targets |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118700156A true CN118700156A (en) | 2024-09-27 |
| CN118700156B CN118700156B (en) | 2025-10-17 |
Family
ID=92813367
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410983876.6A Active CN118700156B (en) | 2024-07-22 | 2024-07-22 | A robotic arm control method for automatic docking of dynamic targets |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118700156B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN119635663A (en) * | 2025-01-21 | 2025-03-18 | 华中科技大学 | A method and system for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance |
| CN119658673A (en) * | 2024-10-18 | 2025-03-21 | 中国科学院长春光学精密机械与物理研究所 | Mechanical arm vision servo tracking method based on self-adaptive neural network compensation |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106485746A (en) * | 2016-10-17 | 2017-03-08 | 广东技术师范学院 | Visual servo mechanical hand based on image no demarcation and its control method |
| CN112847334A (en) * | 2020-12-16 | 2021-05-28 | 北京无线电测量研究所 | Mechanical arm target tracking method based on visual servo |
| CN115524969A (en) * | 2022-09-26 | 2022-12-27 | 西北工业大学 | A Method of Improving the Calculation Speed of Predictive Control of Space Rendezvous and Docking Model |
| US20230042756A1 (en) * | 2021-10-09 | 2023-02-09 | Southeast University | Autonomous mobile grabbing method for mechanical arm based on visual-haptic fusion under complex illumination condition |
| CN116700288A (en) * | 2023-07-11 | 2023-09-05 | 西南大学 | A local trajectory planning method and system for a mobile robot based on adaptive model predictive control |
| WO2023168849A1 (en) * | 2022-03-08 | 2023-09-14 | 江南大学 | Mechanical arm motion capture method, medium, electronic device, and system |
| US20230360254A1 (en) * | 2020-12-31 | 2023-11-09 | Huawei Technologies Co., Ltd. | Pose estimation method and related apparatus |
| US20240009841A1 (en) * | 2022-07-08 | 2024-01-11 | Ubtech Robotics Corp Ltd | Dynamic target tracking method, robot and computer-readable storage medium |
| WO2024027647A1 (en) * | 2022-08-02 | 2024-02-08 | 深圳微美机器人有限公司 | Robot control method and system and computer program product |
| CN118288294A (en) * | 2024-05-14 | 2024-07-05 | 西南交通大学 | A robot visual servo and human-machine collaborative control method based on image variable admittance |
-
2024
- 2024-07-22 CN CN202410983876.6A patent/CN118700156B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106485746A (en) * | 2016-10-17 | 2017-03-08 | 广东技术师范学院 | Visual servo mechanical hand based on image no demarcation and its control method |
| CN112847334A (en) * | 2020-12-16 | 2021-05-28 | 北京无线电测量研究所 | Mechanical arm target tracking method based on visual servo |
| US20230360254A1 (en) * | 2020-12-31 | 2023-11-09 | Huawei Technologies Co., Ltd. | Pose estimation method and related apparatus |
| US20230042756A1 (en) * | 2021-10-09 | 2023-02-09 | Southeast University | Autonomous mobile grabbing method for mechanical arm based on visual-haptic fusion under complex illumination condition |
| WO2023168849A1 (en) * | 2022-03-08 | 2023-09-14 | 江南大学 | Mechanical arm motion capture method, medium, electronic device, and system |
| US20240009841A1 (en) * | 2022-07-08 | 2024-01-11 | Ubtech Robotics Corp Ltd | Dynamic target tracking method, robot and computer-readable storage medium |
| WO2024027647A1 (en) * | 2022-08-02 | 2024-02-08 | 深圳微美机器人有限公司 | Robot control method and system and computer program product |
| CN115524969A (en) * | 2022-09-26 | 2022-12-27 | 西北工业大学 | A Method of Improving the Calculation Speed of Predictive Control of Space Rendezvous and Docking Model |
| CN116700288A (en) * | 2023-07-11 | 2023-09-05 | 西南大学 | A local trajectory planning method and system for a mobile robot based on adaptive model predictive control |
| CN118288294A (en) * | 2024-05-14 | 2024-07-05 | 西南交通大学 | A robot visual servo and human-machine collaborative control method based on image variable admittance |
Non-Patent Citations (1)
| Title |
|---|
| 赵聪慧;赵长海;朱明超;: "基于重力势能的模块化机械臂构型设计", 机电工程, no. 10, 20 October 2020 (2020-10-20) * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN119658673A (en) * | 2024-10-18 | 2025-03-21 | 中国科学院长春光学精密机械与物理研究所 | Mechanical arm vision servo tracking method based on self-adaptive neural network compensation |
| CN119635663A (en) * | 2025-01-21 | 2025-03-18 | 华中科技大学 | A method and system for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118700156B (en) | 2025-10-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110076772B (en) | Grabbing method and device for mechanical arm | |
| CN118700156B (en) | A robotic arm control method for automatic docking of dynamic targets | |
| Salaris et al. | Online optimal perception-aware trajectory generation | |
| Sauvée et al. | Image based visual servoing through nonlinear model predictive control | |
| CN117207190B (en) | Accurate robot system that snatchs based on vision and sense of touch fuse | |
| CN114111772B (en) | A position tracking method for underwater robot software operating hand based on data gloves | |
| CN106406098B (en) | A Human-Computer Interaction Control Method of Robot System in Unknown Environment | |
| Shi et al. | Adaptive image-based visual servoing for hovering control of quad-rotor | |
| CN117901090B (en) | Visual servo method and system for lens holding robot | |
| Han et al. | Visual servoing control of robotics with a neural network estimator based on spectral adaptive law | |
| CN120472324B (en) | A lightweight container correction method and system based on machine vision | |
| Lippiello et al. | 3D monocular robotic ball catching | |
| CN109542094B (en) | Visual Stabilization Control of Mobile Robots with Unexpected Images | |
| CN118991325A (en) | Vehicle suspension control method and device, electronic equipment and storage medium | |
| Taherian et al. | Image-based visual servoing improvement through utilization of adaptive control gain and pseudo-inverse of the weighted mean of the Jacobians | |
| Ge et al. | Binocular vision calibration and 3D re-construction with an orthogonal learning neural network | |
| CN117270398A (en) | Robot gait planning algorithm based on neural network and reinforcement learning | |
| Sepahvand et al. | Deep visual servoing of an aerial robot using keypoint feature extraction | |
| Mohebbi et al. | A comparative study of eye-in-hand image-based visual servoing: Stereo vs. mono | |
| JP2778430B2 (en) | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision | |
| CN118567351B (en) | Real-time tracking data guided learning control method for unmanned autonomous vehicle | |
| CN118700157B (en) | A robotic arm control method for automatic docking of static targets | |
| Lin | Robotic manipulation based on 3d vision: A survey | |
| Tselishchev et al. | Autonomous Navigation of a Mobile Robot Using Overhead Camera and Computer Vision Methods for Time-Critical Tasks | |
| Keshmiri | Image based visual servoing using trajectory planning and augmented visual servoing controller |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |