Moving target-oriented mechanical arm pose tracking control method
By using a robust adaptive control algorithm based on quaternions, the problems of accuracy and speed in pose tracking control of robotic arms in complex dynamic environments were solved, achieving high-precision pose tracking and attitude synchronization, and improving the working efficiency of robotic arms.
Patent Information
- Application Number
- CN202410984221.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-22
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-22
AI Technical Summary
Existing technologies limit the accuracy and response speed of pose control when a robotic arm faces a moving target, especially in complex dynamic environments where high-precision pose tracking control is difficult to achieve.
A robust adaptive control algorithm based on quaternions is adopted. By establishing the second-order pose dynamic equations of the target rigid body and the robotic arm, a robust adaptive controller is designed. An adaptive law is introduced to estimate the upper bound of unknown disturbances, compensate for the influence of external disturbances, and realize the target feature point position tracking and attitude synchronization.
Under conditions where the robotic arm and the rigid body of the moving target are subjected to external disturbances, high-precision pose control, target feature point position tracking and attitude synchronization are achieved, ensuring the stability and response speed of the control system in complex environments.
Smart Images

Figure CN118906051A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the control of an autonomous moving body in a three-dimensional space and the field of robots, and in particular to a robot arm position tracking control method oriented to a moving target. Background Art
[0002] In modern industrial automation, robotic arms are increasingly used, especially in welding, assembly, handling and other work environments that require high precision and high efficiency. However, when a robotic arm faces a moving target, the accuracy and response speed of its posture control are often limited. Therefore, how to achieve high-precision posture tracking control of the robotic arm in a dynamic environment has become a problem that needs to be solved urgently.
[0003] Classic rigid body posture description methods include Euler angles, rotation matrices, modified Rodriguez parameters (MRP) and quaternions. Euler angles are relatively widely used in stabilization problems, but it is difficult to handle tasks that require large-angle rotation and high-precision tracking. Euler angles ignore the coupling between rotation and translation. When the angle is greater than 90 degrees, there will be a many-to-one mapping between Euler angles and rotation matrices. Using Euler angles to describe posture may produce singular values. The method based on Euler angles is relatively mature in theory, but in practical applications it is easily affected by the gimbal lock problem, resulting in poor continuity and stability of control. The rotation matrix requires a large number of quite complex trigonometric functions, which poses certain challenges to the computing performance of the processor. In contrast, the method based on quaternions can avoid the gimbal lock problem and performs better in terms of computing efficiency and stability. Therefore, the present invention uses quaternions to describe the posture of moving targets and manipulators.
[0004] In the posture tracking control of moving targets, the fusion application of visual feedback technology and sensor technology is crucial. By acquiring real-time motion information of the target through sensors such as cameras, combined with image processing and machine learning algorithms, real-time recognition and tracking of the target can be achieved. However, in complex dynamic environments, achieving high-precision posture tracking still faces many challenges, including how to improve the speed and accuracy of target recognition, and how to maintain stable control performance when the robot arm moves at high speed.
[0005] In addition, traditional control algorithms have high computational complexity and poor real-time performance when processing high-dimensional posture data. The quaternion-based control method can effectively reduce the computational complexity and improve the response speed of the control system due to its compact representation and good numerical stability. Therefore, using quaternions to describe posture and study the posture tracking control method of the manipulator for moving targets has important theoretical significance and application value. Summary of the invention
[0006] In order to solve the technical problems existing in the prior art, the embodiment of the present invention provides a robot arm posture tracking control method for a moving target. By combining known target posture information and advanced robust adaptive control algorithms, high-precision and high-response speed posture control can be achieved in a complex dynamic environment, thereby significantly improving the work efficiency and operation capacity of the robot arm. Based on quaternions, the present invention studies the control problem of position tracking and posture synchronization of both the target rigid body and the robot arm joints subject to unknown disturbances. The technical solution is as follows:
[0007] A robot arm posture tracking control method for a moving target, comprising:
[0008] S1. Establish the second-order posture dynamics equation of the target rigid body according to the Newton-Euler method;
[0009] S2. Establish the second-order posture dynamic equation of the end effector of the robot arm according to the Lagrange equation;
[0010] S3, obtaining a relative posture dynamics equation according to the second-order posture dynamics equation of the target rigid body and the second-order posture dynamics equation of the end effector of the manipulator;
[0011] S4. Design a robust adaptive controller for the relative posture dynamics equation.
[0012] Optionally, the second-order posture dynamics equation of the target rigid body is established in S1 according to the Newton-Euler method:
[0013] S101. Define inertial coordinate system O I -X I Y I Z I , the fixed coordinate system of the moving target O T -X T Y T Z T and the fixed coordinate system O at the end of the robot arm E -X E Y E Z E , wherein the origin of the inertial coordinate system is the base point of the robot arm base, the origin of the fixed coordinate system of the moving target is the target center of mass, and the origin of the fixed coordinate system of the end of the robot arm is the end of the robot arm;
[0014] S102. Define the position kinematics of the target rigid body as formula (1):
[0015]
[0016] Among them, p t =[p tx p ty ptz ] T , is the target rigid body in the inertial coordinate system O I The position below;
[0017] v t =[v tx v ty v tz ] T is the target rigid body in the inertial coordinate system O I The speed of the next
[0018] S103. Define the attitude kinematics of the target rigid body as formula (2):
[0019]
[0020] in, is the attitude quaternion of the target rigid body;
[0021] Yes tv A skew-symmetric matrix;
[0022] E(q t ) satisfies E(q t ) T E(q t )=I3,E(q t ) T q t =0;
[0023] ω t =[ω tx ω ty ω tz ] T is the target rigid body in the inertial coordinate system O I Angular velocity under
[0024] S104, according to the position kinematics of the target rigid body and the attitude kinematics of the target rigid body, obtain the position kinematics of the target rigid body, the position kinematics of the target rigid body is formula (3):
[0025]
[0026] in,
[0027] S105, based on Newton's second law, defined in the inertial coordinate system O I The position dynamics of the target rigid body under is formula (4):
[0028]
[0029] Among them, m tis the mass of the target rigid body, f t is the external force on the target rigid body;
[0030] S106, based on the rigid body Euler dynamics equation, defined in the inertial coordinate system O I The attitude dynamics of the target rigid body under is formula (5):
[0031]
[0032] J t is the moment of inertia of the target rigid body, τ t is the external torque on the target rigid body;
[0033] S107, deriving the position and posture kinematics of the target rigid body, combining the position dynamics of the target rigid body and the attitude dynamics of the target rigid body, to obtain a second-order position and posture dynamics equation of the target rigid body, the second-order position and posture dynamics equation of the target rigid body is formula (6):
[0034]
[0035] When the target rigid body is subjected to external force f t and torque τ t They are all unknown disturbance signals, but the disturbance has an upper bound and satisfies h is estimated by an adaptive law.
[0036] Optionally, the second-order posture dynamic equation of the end effector of the manipulator is established according to the Lagrange equation in S2, including:
[0037] S201. A dynamic model of an n-joint robotic arm in a joint space is established according to the Lagrange equation. The dynamic model of an n-joint robotic arm in a joint space is formula (7):
[0038]
[0039] in, is the joint angle of the robot arm;
[0040] is the joint angular velocity;
[0041] is the joint angular acceleration;
[0042] is the inertia matrix of the robot;
[0043] is the centrifugal Coriolis force matrix of the robot arm;
[0044] is the gravitational moment term of the manipulator;
[0045] is the friction torque term of the robot arm;
[0046] is the torque of each joint of the robot arm;
[0047] d is the unknown external interference torque, but d has an upper bound, that is, Estimation through adaptive law;
[0048] S202, when the posture of the end of the robot arm is described based on the Euler angle, the differential kinematics of the posture of the end of the robot arm is formula (8);
[0049]
[0050] in, is the position of the end of the robotic arm in the inertial coordinate system;
[0051] is the angular velocity of the end of the robotic arm in the inertial coordinate system;
[0052] is the Jacobian matrix of the robot arm;
[0053] is the position component of the Jacobian matrix of the robot;
[0054] is the posture component of the Jacobian matrix of the robot;
[0055] S203. When quaternions are used to describe the end posture, the differential kinematics of the end posture of the manipulator is formula (10):
[0056]
[0057] in, Represents the attitude quaternion of the end of the robotic arm in the inertial coordinate system;
[0058] Represents the Jacobian matrix of the robot arm when the posture of the end of the robot arm is described by quaternion;
[0059] represents the attitude component of the Jacobian matrix;
[0060] S204, the kinematic relationship between the derivative of the quaternion of the robot end attitude and the angular velocity satisfies formula (10):
[0061]
[0062] in,
[0063] Yesv A skew-symmetric matrix;
[0064] E(q) satisfies E(q) T E(q)=I3,E(q) T q=0;
[0065] S204, based on the kinematic relationship between the derivative of the quaternion of the manipulator end posture and the angular velocity, and, get and, 2E(q) T J q =J ω ;
[0066] Based on the differential kinematics of the end-position of the manipulator, we get
[0067] If the robot arm is not singular, then J A Full rank, then J A The row rank of is equal to the column rank;
[0068] If the robot arm is singular, then the corresponding differential inverse kinematics has no solution;
[0069] S205, will and The dynamic model of the n-joint manipulator in the joint space is brought into play to obtain the dynamic model of the end of the manipulator in the task space. The dynamic model of the end of the manipulator in the task space is given by formula (11):
[0070]
[0071] S206, deforming the dynamic model of the end of the robot arm in the task space to obtain formula (12):
[0072]
[0073] in,
[0074] S207. Based on the dynamic model of the deformed end of the robotic arm in the task space, a second-order posture dynamic equation of the end effector of the robotic arm is obtained. The second-order posture dynamic equation of the end effector of the robotic arm is formula (13):
[0075]
[0076] Optionally, the relative posture dynamics equation obtained in S3 according to the second-order posture dynamics equation of the target rigid body and the second-order posture dynamics equation of the end effector of the manipulator comprises:
[0077] S301, define a posture tracking error, wherein the posture tracking error is formula (14):
[0078] r e =r t -r; (14)
[0079] S302, calculating the second-order derivative of the posture tracking error to obtain a relative posture dynamics equation, which is formula (15):
[0080]
[0081] Optionally, the step S4 of designing a robust adaptive controller for the relative posture dynamics equation comprises:
[0082] S401, define a sliding surface and obtain a sliding surface formula;
[0083] S402, deriving the sliding surface formula;
[0084] S403, defining a Lyapunov function, and obtaining a Lyapunov function formula;
[0085] S404, deriving the Lyapunov function formula;
[0086] S405. Design a controller and an adaptive law, and obtain expressions of the controller and the adaptive law;
[0087] S406. Based on the controller, the expression of the adaptive law and the result of the derivation of the Lyapunov function formula, the robot arm joint torque required to complete the trajectory tracking control task is transformed.
[0088] Optionally, the sliding surface formula in S401 is formula (16):
[0089]
[0090] Among them, Λ=diag{λ1, λ2,..., λ7}, λ i >0, i=1,2,…,7.
[0091] Optionally, the formula for deriving the sliding surface formula in S402 is formula (17):
[0092]
[0093] Optionally, the Lyapunov function formula in S403 is formula (18):
[0094]
[0095] Where V is a positive definite Lyapunov scalar function, is the perturbation upper bound estimation error, is the estimated value of h; is the upper bound estimation error of the disturbance torque, yes ; γ and β are both normal numbers.
[0096] Optionally, the formula for deriving the Lyapunov function formula in S404 is formula (19);
[0097]
[0098] Optionally, the expression of the controller in S405 is formula (20)
[0099]
[0100] The result of the derivative of the controller, the expression of the adaptive law and the Lyapunov function formula in S406 is transformed to obtain the mechanical arm joint torque required to complete the trajectory tracking control task:
[0101] S4061. Combining formula (20) with formula (19) to obtain formula (21):
[0102]
[0103] The adaptive law is formula (22):
[0104]
[0105] Among them, ||s||1 is the norm of s;
[0106] S4062. Combining formula (22) with formula (21), we obtain formula (23):
[0107]
[0108] S4063, according to The joint torque of the manipulator required to complete the trajectory tracking control task is obtained. The joint torque of the manipulator required to complete the trajectory tracking control task is formula (24):
[0109]
[0110] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:
[0111] In the above scheme, the relative posture modeling and coordinated control of the robot arm and the moving target rigid body in three-dimensional space are studied. By establishing a relative posture dynamics model between the feature points of the two rigid bodies, a robust adaptive relative posture tracking control method based on quaternions is proposed, which can realize the target feature point position tracking and posture synchronization under the condition that both the moving target rigid body and the robot arm joints are subject to external disturbances. In order to solve the problem of unknown interference to the moving target rigid body and the robot arm joints, the present invention introduces an adaptive law to estimate the upper bound of the unknown interference, and introduces a robust term in the controller to compensate for the influence of external unknown interference on the control system. Under the Lyapunov framework, by adjusting the design parameters, it is strictly proved that the relative position tracking error and posture synchronization error between the two feature points converge to a small neighborhood of zero. BRIEF DESCRIPTION OF THE DRAWINGS
[0112] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.
[0113] Figure 1 It is a flow chart of a method for tracking and controlling a position and posture of a manipulator facing a moving target provided by an embodiment of the present invention;
[0114] Figure 2 is a schematic diagram of a scene in which a robotic arm tracks a moving target rigid body in an embodiment of the present invention;
[0115] Figure 3 is a graph showing the relative position and posture of the moving target rigid body and the end of the robotic arm changing over time in an embodiment of the present invention;
[0116] Figure 4 is a graph showing the variation of the derivative of the relative position of the moving target rigid body and the end of the robotic arm over time in an embodiment of the present invention;
[0117] Figure 5 is a graph showing changes in the control torque of the robot joint over time in an embodiment of the present invention;
[0118] Figure 6 is a curve diagram showing the estimated value of the upper bound of the unknown disturbance to the target rigid body in the embodiment of the present invention changing with time;
[0119] Figure 7 It is a curve diagram showing the estimated value of the upper limit of the unknown disturbance torque applied to the manipulator in the embodiment of the present invention changing with time. DETAILED DESCRIPTION
[0120] In order to make the purpose, technical solution and advantages of the embodiment of the present invention clearer, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings of the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all of the embodiments. Based on the described embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0121] Unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Similarly, words such as "one", "one" or "the" do not indicate a quantitative limitation, but indicate the existence of at least one. Words such as "include" or "comprise" mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. Words such as "connect" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect.
[0122] It should be noted that the terms "up", "down", "left", "right", "front", "back", etc. used in the present invention are only used to indicate relative position relationships. When the absolute position of the object being described changes, the relative position relationship may also change accordingly.
[0123] In control system design, the existence of unknown disturbances is a common and unavoidable problem. In order to deal with these disturbances, robust adaptive control technology is widely used. Robust control refers to a type of control method that can still guarantee system performance and stability when there are uncertainties in system parameters and external disturbances. By designing the adaptive rate, the uncertain model parameters and the upper bound of unknown disturbances can be estimated. By introducing robust terms in the controller, these uncertainties can be effectively handled, thereby improving the robustness of the system and preventing the system from losing control or failing due to external disturbances. By effectively handling unknown disturbances, the system can maintain excellent performance under various working conditions, such as fast response, low steady-state error and small overshoot. In applications with extremely high reliability requirements such as aerospace and autonomous driving, robust control can ensure the continuous and stable operation of the system in complex environments and improve overall reliability. By introducing robust terms and adaptive rates, the stability and reliability of the system are improved, so that the control system can still operate efficiently and stably in various complex and changeable environments.
[0124] like Figures 1 to 7As shown, this embodiment provides a robot arm posture tracking control method for a moving target, and the specific scheme includes:
[0125] S1. Establish the second-order posture dynamics equation of the target rigid body according to the Newton-Euler method;
[0126] S2. Establish the second-order posture dynamic equation of the end effector of the robot arm according to the Lagrange equation;
[0127] S3, obtaining a relative posture dynamics equation according to the second-order posture dynamics equation of the target rigid body and the second-order posture dynamics equation of the end effector of the manipulator;
[0128] S4. Design a robust adaptive controller for the relative posture dynamics equation.
[0129] In a specific implementation manner, the second-order posture dynamics equation of the target rigid body is established in S1 according to the Newton-Euler method:
[0130] S101. Define inertial coordinate system O I -X I Y I Z I , the fixed coordinate system of the moving target O T -X T Y T Z T and the fixed coordinate system O at the end of the robot arm E -X E Y E Z E , wherein the origin of the inertial coordinate system is the base point of the robot arm base, the origin of the fixed coordinate system of the moving target is the target center of mass, and the origin of the fixed coordinate system of the end of the robot arm is the end of the robot arm;
[0131] S102. Define the position kinematics of the target rigid body as formula (1):
[0132]
[0133] Among them, p t =[p tx p ty p tz ] T , is the target rigid body in the inertial coordinate system O I The position below;
[0134] v t =[v tx v ty v tz ] T is the target rigid body in the inertial coordinate system O IThe speed of the next
[0135] S103. Define the attitude kinematics of the target rigid body as formula (2):
[0136]
[0137] in, is the attitude quaternion of the target rigid body;
[0138] Yes tv A skew-symmetric matrix;
[0139] E(q t ) satisfies E(q t ) T E(q t )=I3,E(q t ) T q t =0;
[0140] ω t =[ω tx ω ty ω tz ] T is the target rigid body in the inertial coordinate system O I Angular velocity under
[0141] S104, according to the position kinematics of the target rigid body and the attitude kinematics of the target rigid body, obtain the position kinematics of the target rigid body, the position kinematics of the target rigid body is formula (3):
[0142]
[0143] in,
[0144] S105, based on Newton's second law, defined in the inertial coordinate system O I The position dynamics of the target rigid body under is formula (4):
[0145]
[0146] Among them, m t is the mass of the target rigid body, f t is the external force on the target rigid body;
[0147] S106, based on the rigid body Euler dynamics equation, defined in the inertial coordinate system O I The attitude dynamics of the target rigid body under is formula (5):
[0148]
[0149] Jt is the moment of inertia of the target rigid body, τ t is the external torque on the target rigid body;
[0150] S107, deriving the position and posture kinematics of the target rigid body, combining the position dynamics of the target rigid body and the attitude dynamics of the target rigid body, to obtain a second-order position and posture dynamics equation of the target rigid body, the second-order position and posture dynamics equation of the target rigid body is formula (6):
[0151]
[0152] When the target rigid body is subjected to external force f t and torque τ t They are all unknown disturbance signals, but the disturbance has an upper bound and satisfies h is estimated by an adaptive law.
[0153] In a specific implementation manner, the second-order posture dynamic equation of the end effector of the manipulator is established according to the Lagrange equation in S2, including:
[0154] S201. A dynamic model of an n-joint robotic arm in a joint space is established according to the Lagrange equation. The dynamic model of an n-joint robotic arm in a joint space is formula (7):
[0155]
[0156] in, is the joint angle of the robot arm;
[0157] is the joint angular velocity;
[0158] is the joint angular acceleration;
[0159] is the inertia matrix of the robot;
[0160] is the centrifugal Coriolis force matrix of the robot arm;
[0161] is the gravitational moment term of the manipulator;
[0162] is the friction torque term of the robot arm;
[0163] is the torque of each joint of the robot arm;
[0164] d is the unknown external interference torque, but d has an upper bound, that is, Estimation through adaptive law;
[0165] S202, when the posture of the end of the robot arm is described based on the Euler angle, the differential kinematics of the posture of the end of the robot arm is formula (8);
[0166]
[0167] in, is the position of the end of the robotic arm in the inertial coordinate system;
[0168] is the angular velocity of the end of the robotic arm in the inertial coordinate system;
[0169] is the Jacobian matrix of the robot arm;
[0170] is the position component of the Jacobian matrix of the robot;
[0171] is the posture component of the Jacobian matrix of the robot;
[0172] S203. When quaternions are used to describe the end posture, the differential kinematics of the end posture of the manipulator is formula (10):
[0173]
[0174] in, Represents the attitude quaternion of the end of the robotic arm in the inertial coordinate system;
[0175] Represents the Jacobian matrix of the robot arm when the posture of the end of the robot arm is described by quaternion;
[0176] represents the attitude component of the Jacobian matrix;
[0177] S204. The kinematic relationship between the derivative of the quaternion of the robot end attitude and the angular velocity satisfies formula (10):
[0178]
[0179] in,
[0180] Yes v A skew-symmetric matrix;
[0181] E(q) satisfies E(q) T E(q)=I3,E(q) T q=0;
[0182] S204, based on the kinematic relationship between the derivative of the quaternion of the manipulator end posture and the angular velocity, and, get and, 2E(q) T J q =J ω ;
[0183] Based on the differential kinematics of the end-position of the manipulator, we get
[0184] If the robot arm is not singular, then J A Full rank, then J A The row rank of is equal to the column rank;
[0185] If the robot arm is singular, then the corresponding differential inverse kinematics has no solution;
[0186] S205, will and The dynamic model of the n-joint manipulator in the joint space is brought into play to obtain the dynamic model of the end of the manipulator in the task space. The dynamic model of the end of the manipulator in the task space is given by formula (11):
[0187]
[0188] S206, deforming the dynamic model of the end of the robot arm in the task space to obtain formula (12):
[0189]
[0190] in,
[0191] S207. Based on the dynamic model of the deformed end of the robotic arm in the task space, a second-order posture dynamic equation of the end effector of the robotic arm is obtained. The second-order posture dynamic equation of the end effector of the robotic arm is formula (13):
[0192]
[0193] In a specific implementation manner, the step S3 obtains the relative posture dynamics equation according to the second-order posture dynamics equation of the target rigid body and the second-order posture dynamics equation of the end effector of the manipulator, including:
[0194] S301, define a posture tracking error, wherein the posture tracking error is formula (14):
[0195] r e =r t -r; (14)
[0196] S302, calculating the second-order derivative of the posture tracking error to obtain a relative posture dynamics equation, which is formula (15):
[0197]
[0198] In a specific implementation manner, the step S4 of designing a robust adaptive controller for the relative posture dynamics equation includes:
[0199] S401, define a sliding surface and obtain a sliding surface formula; the sliding surface formula is formula (16):
[0200]
[0201] Among them, Λ=diag{λ1, λ2,..., λ7}, λ i >0, i=1,2,…,7.
[0202] S402, deriving the sliding surface formula; the formula for deriving the sliding surface formula is formula (17):
[0203]
[0204] S403, define the Lyapunov function to obtain the Lyapunov function formula; the Lyapunov function formula is formula (18):
[0205]
[0206] Where V is a positive definite Lyapunov scalar function, is the perturbation upper bound estimation error, is the estimated value of h; is the upper bound estimation error of the disturbance torque, yes ; γ and β are both normal numbers.
[0207] S404, deriving the Lyapunov function formula; the formula for deriving the Lyapunov function formula is formula (19);
[0208]
[0209] S405. Design a controller and an adaptive law, and obtain expressions of the controller and the adaptive law;
[0210] The expression of the controller in S405 is formula (20)
[0211]
[0212] S406, based on the definition of the controller and the result of the derivation of the Lyapunov function formula, transforming to obtain the mechanical arm joint torque required to complete the trajectory tracking control task, including the following steps:
[0213] S4061. Combining formula (20) with formula (19) to obtain formula (21):
[0214]
[0215] The adaptive law is formula (22):
[0216]
[0217] Among them, ||s||1 is the norm of s;
[0218] S4062. Combining formula (22) with formula (21), we obtain formula (23):
[0219]
[0220] S4063, according to The joint torque of the manipulator required to complete the trajectory tracking control task is obtained. The joint torque of the manipulator required to complete the trajectory tracking control task is formula (24):
[0221]
[0222] Computer numerical simulation is used to verify the effectiveness of the motion target-oriented robot arm posture tracking control method provided in this embodiment, and the simulation platform is based on Matlab software under Win11x64 operating system.
[0223] In a specific implementation, the physical parameters of the moving target rigid body are considered to be:
[0224]
[0225] m t =1(kg)
[0226] The initial position vector and attitude quaternion of the target rigid body are p t (0) = [0.8179 1.0845 0] T and q t (0) = [0.9986 0 0 -0.053] T The external interference force f on the target rigid body t =[ab 0] T (N), external disturbance torque τ t =[00 c] T(N·m), where a, b, c are random values between -0.00001 and 0.00001.
[0227] A two-link robotic arm is used for simulation verification. The physical parameters of the robotic arm are:
[0228]
[0229]
[0230] In the above formula, p2=m2l1l2, g=9.81(m / s 2 ), m i and l i is the mass and length of the robot arm link i (i=1,2). Assume m1=1(kg), m2=0.8(kg), l1=1(m), l2=0.8(m). The external disturbance torque on the robot arm is d=[0.0001 0.0001] T (N·m).
[0231] θ and The initial condition is set to θ(0) = [0 0] T (rad), At this time, the initial position vector and attitude quaternion of the end effector of the robot arm are p(0)=[1.8 0 0] T and q(0)=[1 0 0 0] T .
[0232] The initial parameter estimates are set to The adjustable parameters are γ=0.0001, β=0.00001, Λ=diag{1 1 1 1 1 1 1} and α=0.12.
[0233] The simulation results are as follows Figure 3 , Figure 4 , Figure 5 , Figure 6 and Figure 7 As shown. Among them, Figure 3 and Figure 4 The curves of the pose tracking error and the derivative of the pose tracking error respectively indicate that the relative pose and its derivative of the moving target rigid body and the end of the manipulator converge to a small neighborhood of zero within 80 (s), and the steady-state relative pose error and its derivative error are less than 5×10 -4 and 2×10 -4 , which means that the relative translation and rotation are well controlled, with high precision and fast stability. For the two-link robot, the 3rd, 5th, and 6th elements of the relative posture error and its derivative vector are always 0, so Figure 3 and Figure 4 In the simulation results, only the variation curves of the 1st, 2nd, 4th and 7th elements of the relative posture error and its derivative vector are plotted. Figure 5 It represents the joint output torque of the robot arm, and the torque converges within 80 (s). Due to the friction torque term in the dynamic model of the robot arm, there is still a small vibration after the control torque converges. Figure 6 Represents the upper bound estimate of the unknown disturbance to the target rigid body The change curve. Figure 7 Represents the upper bound estimate of the unknown disturbance torque on the manipulator The effectiveness of the proposed control strategy is verified through simulation.
[0234] The following points need to be explained:
[0235] (1) The drawings of the embodiments of the present invention only relate to the structures related to the embodiments of the present invention, and other structures may refer to the general design.
[0236] (2) For the sake of clarity, in the drawings used to describe the embodiments of the present invention, the thickness of the layers or regions is exaggerated or reduced, that is, these drawings are not drawn according to the actual scale. It is understood that when an element such as a layer, film, region or substrate is referred to as being "on" or "under" another element, the element may be "directly" "on" or "under" the other element or there may be intermediate elements.
[0237] (3) In the absence of conflict, the embodiments of the present invention and the features therein may be combined with each other to obtain new embodiments.
[0238] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. The protection scope of the present invention shall be based on the protection scope of the claims.
Claims
1. A robot arm posture tracking control method for a moving target, characterized in that: include: S1. Establish the second-order posture dynamics equation of the target rigid body according to the Newton-Euler method; S2. Establish the second-order posture dynamic equation of the end effector of the robot arm according to the Lagrange equation; S3, obtaining a relative posture dynamics equation according to the second-order posture dynamics equation of the target rigid body and the second-order posture dynamics equation of the end effector of the manipulator; S4. Design a robust adaptive controller for the relative posture dynamics equation.
2. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 1, characterized in that: The second-order posture dynamics equation of the target rigid body is established in S1 according to the Newton-Euler method: S101. Define inertial coordinate system O I -X I Y I Z I , the fixed coordinate system of the moving target O T -X T Y T Z T and the fixed coordinate system O at the end of the robot arm E -X E Y E Z E , wherein the origin of the inertial coordinate system is the base point of the robot arm base, the origin of the fixed coordinate system of the moving target is the target center of mass, and the origin of the fixed coordinate system of the end of the robot arm is the end of the robot arm; S102. Define the position kinematics of the target rigid body as formula (1): Among them, p t =[p tx p ty p tz ] T , is the target rigid body in the inertial coordinate system O I The position below; v t =[v tx v ty v tz ] T is the target rigid body in the inertial coordinate system O I The speed of the next S103. Define the attitude kinematics of the target rigid body as formula (2): in, is the attitude quaternion of the target rigid body; Yes tv A skew-symmetric matrix; E(q t ) satisfies E(q t ) T E(q t )=I3,E(q t ) T q t =0; ω t =[ω tx ω ty ω tz ] T is the target rigid body in the inertial coordinate system O I Angular velocity under S104, according to the position kinematics of the target rigid body and the attitude kinematics of the target rigid body, obtain the position kinematics of the target rigid body, the position kinematics of the target rigid body is formula (3): in, S105, based on Newton's second law, defined in the inertial coordinate system O I The position dynamics of the target rigid body under is formula (4): Among them, m t is the mass of the target rigid body, f t is the external force on the target rigid body; S106, based on the rigid body Euler dynamics equation, defined in the inertial coordinate system O I The attitude dynamics of the target rigid body under is formula (5): J t is the moment of inertia of the target rigid body, τ t is the external torque on the target rigid body; S107, deriving the position and posture kinematics of the target rigid body, combining the position dynamics of the target rigid body and the attitude dynamics of the target rigid body, to obtain a second-order position and posture dynamics equation of the target rigid body, the second-order position and posture dynamics equation of the target rigid body is formula (6): When the target rigid body is subjected to external force f t and torque τ t They are all unknown disturbance signals, but the disturbance has an upper bound and satisfies h is estimated by an adaptive law.
3. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 2, characterized in that: The second-order posture dynamic equation of the end effector of the manipulator established in S2 according to the Lagrange equation includes: S201. A dynamic model of an n-joint robotic arm in a joint space is established according to the Lagrange equation. The dynamic model of an n-joint robotic arm in a joint space is formula (7): in, is the joint angle of the robot arm; is the joint angular velocity; is the joint angular acceleration; is the inertia matrix of the robot; is the centrifugal Coriolis force matrix of the robot arm; is the gravitational moment term of the manipulator; is the friction torque term of the robot arm; is the torque of each joint of the robot arm; d is the unknown external interference torque, but d has an upper bound, that is, Estimation through adaptive law; S202, when the posture of the end of the robot arm is described based on the Euler angle, the differential kinematics of the posture of the end of the robot arm is formula (8); in, is the position of the end of the robotic arm in the inertial coordinate system; is the angular velocity of the end of the robotic arm in the inertial coordinate system; is the Jacobian matrix of the robot arm; is the position component of the Jacobian matrix of the robot; is the posture component of the Jacobian matrix of the robot; S203. When quaternions are used to describe the end posture, the differential kinematics of the end posture of the manipulator is formula (10): in, Represents the attitude quaternion of the end of the robotic arm in the inertial coordinate system; Represents the Jacobian matrix of the robot arm when the posture of the end of the robot arm is described by quaternion; represents the attitude component of the Jacobian matrix; S204, the kinematic relationship between the derivative of the quaternion of the robot end attitude and the angular velocity satisfies formula (10): in, Yes v A skew-symmetric matrix; E(q) satisfies E(q) T E(q)=I3,E(q) T q=0; S204, based on the kinematic relationship between the derivative of the quaternion of the robot end posture and the angular velocity, and, get and, 2E(q) T J q =J ω ; Based on the differential kinematics of the end-position of the manipulator, we get If the robot arm is not singular, then J A Full rank, then J A The row rank of is equal to the column rank; If the robot arm is singular, then the corresponding differential inverse kinematics has no solution; S205, will and The dynamic model of the n-joint manipulator in the joint space is brought into play to obtain the dynamic model of the end of the manipulator in the task space. The dynamic model of the end of the manipulator in the task space is given by formula (11): S206, deforming the dynamic model of the end of the robot arm in the task space to obtain formula (12): in, S207. Based on the dynamic model of the deformed end of the robotic arm in the task space, a second-order posture dynamic equation of the end effector of the robotic arm is obtained. The second-order posture dynamic equation of the end effector of the robotic arm is formula (13):
4. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 3, characterized in that: The relative posture dynamics equation obtained in S3 according to the second-order posture dynamics equation of the target rigid body and the second-order posture dynamics equation of the end effector of the manipulator includes: S301, define a posture tracking error, wherein the posture tracking error is formula (14): r e =r t -r;(14) S302, calculating the second-order derivative of the posture tracking error to obtain a relative posture dynamics equation, which is formula (15):
5. The method for tracking and controlling a manipulator posture of a moving target according to claim 4, characterized in that: The S4 designs a robust adaptive controller for the relative posture dynamic equations, including: S401, define a sliding surface and obtain a sliding surface formula; S402, deriving the sliding surface formula; S403, defining a Lyapunov function, and obtaining a Lyapunov function formula; S404, deriving the Lyapunov function formula; S405. Design a controller and an adaptive law, and obtain expressions of the controller and the adaptive law; S406. Based on the controller, the expression of the adaptive law and the result of the derivation of the Lyapunov function formula, the robot arm joint torque required to complete the trajectory tracking control task is transformed.
6. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 5, characterized in that: The sliding surface formula in S401 is formula (16): Where, Λ=diag{λ1,λ2,…,λ7}, λ i >0, i=1,2,…,7.
7. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 6, characterized in that: The formula for deriving the sliding surface formula in S402 is formula (17):
8. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 7, characterized in that: The Lyapunov function formula in S403 is formula (18): Where V is a positive definite Lyapunov scalar function, is the perturbation upper bound estimation error, is the estimated value of h; is the upper bound estimation error of the disturbance torque, yes ; γ and β are both normal numbers.
9. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 8, characterized in that: The formula for deriving the Lyapunov function formula in S404 is formula (19); 10. The method for tracking and controlling a manipulator position and posture of a moving target according to claim 9, characterized in that: The expression of the controller in S405 is formula (20) The result of the derivative of the controller, the expression of the adaptive law and the Lyapunov function formula in S406 is transformed to obtain the mechanical arm joint torque required to complete the trajectory tracking control task: S4061. Combining formula (20) with formula (19) to obtain formula (21): The adaptive law is formula (22): Among them, ||s||1 is the norm of s; S4062. Combining formula (22) with formula (21), we obtain formula (23): S4063, according to The joint torque of the manipulator required to complete the trajectory tracking control task is obtained. The joint torque of the manipulator required to complete the trajectory tracking control task is formula (24):
Citation Information
Patent Citations
Robot track synchronous control method and computer readable storage medium
CN112847373A
Hydraulic mechanical arm contact operation self-adaptive impedance control system without tail end force sensor
CN114800522A
Mechanical arm system, mechanical arm, control method of mechanical arm, controller and storage medium
CN115890735A
Cited By
Mechanical arm tail end safety control method and system for cultural relic fingerprints
CN120439299A
