A robotic arm pose tracking control method for moving targets

Abstract

This invention provides a robotic arm pose tracking control method for moving targets, relating to the control of autonomous moving bodies in three-dimensional space and the field of robotics. The control method includes: establishing the second-order pose dynamics equation of the target rigid body according to the Newton-Euler method; establishing the second-order pose dynamics equation of the robotic arm end effector according to the Lagrange equation; obtaining the relative pose dynamics equation based on the second-order pose dynamics equations of the target rigid body and the robotic arm end effector; and designing a robust adaptive controller for the relative pose dynamics equation, enabling the robotic arm end effector to complete the pose tracking control task of the moving target rigid body.

Description

Technical Field

[0001] This invention relates to the control of autonomous moving bodies in three-dimensional space and the field of robotics, and in particular to a method for tracking and controlling the pose of a robotic arm oriented towards a moving target. Background Technology

[0002] In modern industrial automation, robotic arms are increasingly widely used, especially in work environments requiring high precision and efficiency, such as welding, assembly, and material handling. However, when a robotic arm faces a moving target, the accuracy and response speed of its pose control are often limited. Therefore, how to achieve high-precision pose tracking control of robotic arms in dynamic environments has become a pressing problem to be solved.

[0003] Classical rigid body attitude description methods include Euler angles, rotation matrices, modified Rodrigues parameters (MRP), and quaternions. Euler angles are widely used in stabilization problems, but they struggle with tasks requiring large-angle rotations and high-precision tracking. Euler angles neglect the coupling between rotation and translation; when the angle exceeds 90 degrees, a many-to-one mapping occurs between Euler angles and rotation matrices, potentially leading to singular values ​​when using Euler angles to describe attitude. While Euler angle-based methods are theoretically mature, they are susceptible to gimbal lock in practical applications, resulting in poor control continuity and stability. Rotation matrices require numerous and complex trigonometric function calculations, posing a challenge to processor performance. In contrast, quaternion-based methods avoid gimbal lock and offer better computational efficiency and stability. Therefore, this invention uses quaternions to describe the attitude of the moving target and the robotic arm.

[0004] In the pose tracking and control of moving targets, the integrated application of visual feedback technology and sensing technology is crucial. By acquiring real-time motion information of the target through sensing devices such as cameras, and combining this information with image processing and machine learning algorithms, real-time target identification and tracking can be achieved. However, achieving high-precision pose tracking in complex dynamic environments still faces many challenges, including how to improve the speed and accuracy of target recognition, and how to maintain stable control performance under high-speed robotic arm movements.

[0005] Furthermore, traditional control algorithms suffer from high computational complexity and poor real-time performance when processing high-dimensional pose data. Quaternion-based control methods, due to their compact representation and good numerical stability, can effectively reduce computational complexity and improve the response speed of the control system. Therefore, using quaternions to describe pose and researching pose tracking control methods for robotic arms targeting moving objects has significant theoretical and practical value. Summary of the Invention

[0006] To address the technical problems existing in the prior art, this invention provides a robotic arm pose tracking and control method for moving targets. By combining known target pose information with an advanced robust adaptive control algorithm, high-precision and high-response pose control can be achieved in complex dynamic environments, thereby significantly improving the working efficiency and operational capabilities of the robotic arm. This invention studies the control problem of position tracking and attitude synchronization of both the target rigid body and the robotic arm joints under unknown disturbances based on quaternions. The technical solution is as follows:

[0007] A robotic arm pose tracking and control method for moving targets includes:

[0008] S1. Establish the second-order pose dynamics equations of the target rigid body according to the Newton-Euler method;

[0009] S2. Establish the second-order pose dynamics equations of the robotic arm end effector based on the Lagrange equations;

[0010] S3. Based on the second-order pose dynamics equation of the target rigid body and the second-order pose dynamics equation of the robotic arm end effector, the relative pose dynamics equation is obtained.

[0011] S4. Design a robust adaptive controller for the relative pose dynamics equation.

[0012] Optionally, the second-order pose dynamics equation of the target rigid body is established according to the Newton-Euler method in S1:

[0013] S101, Define the inertial coordinate system O I -X I Y I Z I The fixed coordinate system O of the moving target T -X T Y T Z T and the fixed coordinate system O at the end of the robotic arm E -X E Y E Z E Wherein, the origin of the inertial coordinate system is the base point of the robotic arm base, the origin of the fixed coordinate system of the moving target is the target centroid, and the origin of the fixed coordinate system of the robotic arm end is the robotic arm end.

[0014] S102. Define the position kinematics of the target rigid body as Equation (1):

[0015]

[0016] Where, p t =[p tx p ty ptz ] T For the target rigid body in the inertial coordinate system O I The lower position;

[0017] v t =[v tx v ty v tz ] T For the target rigid body in the inertial coordinate system O I The speed of the downward movement;

[0018] S103. Define the attitude kinematics of the target rigid body as Equation (2):

[0019]

[0020] in, It is the attitude quaternion of the target rigid body;

[0021] It is q tv 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 For the target rigid body in the inertial coordinate system O I angular velocity at the bottom;

[0024] S104. Based on the position kinematics and attitude kinematics of the target rigid body, the pose kinematics of the target rigid body is obtained, and the pose 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 is given by formula (4):

[0028]

[0029] Where, m tIt is the mass of the target rigid body, f t It is the external force acting on the target rigid body;

[0030] S106. Based on the Euler equations of rigid body dynamics, defined in the inertial coordinate system O I The attitude dynamics of the target rigid body are given by formula (5):

[0031]

[0032] J t It is the moment of inertia of the target rigid body, τ t It is the external torque acting on the target rigid body;

[0033] S107. Differentiate the pose kinematics of the target rigid body, and combine the position dynamics and attitude dynamics of the target rigid body to obtain the second-order pose dynamics equation of the target rigid body, which is Equation (6):

[0034]

[0035] When the target rigid body is subjected to an external force f t and torque τ t All are unknown disturbance signals, but the disturbances have an upper bound and satisfy the following conditions: h is estimated using an adaptive law.

[0036] Optionally, the second-order pose dynamics equations of the robotic arm end effector established by S2 based on the Lagrange equations include:

[0037] S201. Based on the Lagrange equation, establish the dynamic model of the n-joint robotic arm in the joint space. The dynamic model of the n-joint robotic arm in the joint space is Equation (7):

[0038]

[0039] in, For the joint angles of the robotic arm;

[0040] Joint angular velocity;

[0041] Joint angular acceleration;

[0042] Here is the inertia matrix of the robotic arm;

[0043] The Coriolis force matrix represents the centrifugal force of the robotic arm;

[0044] This is the gravitational torque term of the robotic arm;

[0045] This is the frictional torque term of the robotic arm;

[0046] These are the torques of the various joints of the robotic arm;

[0047] d represents the unknown external disturbance torque, but d has an upper bound, i.e. Estimation via adaptive law;

[0048] S202. When describing the end-effector posture based on Euler angles, the differential kinematics of the end-effector posture is given by formula (8).

[0049]

[0050] in, This represents the position of the robotic arm's end effector in the inertial coordinate system.

[0051] The angular velocity of the robotic arm's end effector in the inertial coordinate system;

[0052] For the Jacobian matrix of the robotic arm;

[0053] For the position components of the Jacobian matrix of the robotic arm;

[0054] For the attitude components of the Jacobian matrix of the robotic arm;

[0055] S203. When using quaternions to describe the end-effector posture, the differential kinematics of the robot arm's end-effector posture is given by formula (10):

[0056]

[0057] in, The quaternion represents the attitude of the robotic arm's end effector in the inertial coordinate system;

[0058] This represents the Jacobian matrix of the robotic arm when the end-effector posture is described using quaternions.

[0059] This represents the attitude components of the Jacobian matrix;

[0060] S204. The kinematic relationship between the derivative of the quaternion of the robotic arm's end-effector posture and the angular velocity satisfies formula (10):

[0061]

[0062] in,

[0063] It is qv skew-symmetric matrix;

[0064] E(q) satisfies E(q) T E(q) = I³, E(q) T q = 0;

[0065] S204. Based on the kinematic relationship between the derivative of the quaternion of the robotic arm's end-effector posture and the angular velocity, and... get And, 2E(q) T J q =J ω ;

[0066] Based on the differential kinematics of the robotic arm's end effector pose, we obtain

[0067] If the robotic arm is not singular, then J A If the rank is full, then J A The row rank is equal to the column rank;

[0068] If the robotic arm is singular, then the corresponding differential inverse kinematics has no solution;

[0069] S205, will and Substituting the dynamic model of the n-joint robotic arm in the joint space, we obtain the dynamic model of the robotic arm end effector in the task space, which is Equation (11):

[0070]

[0071] S206. The dynamic model of the end effector of the robotic arm in the task space is deformed to obtain formula (12):

[0072]

[0073] in,

[0074] S207. Based on the dynamic model of the deformed end effector in the task space, the second-order pose dynamic equation of the end effector is obtained, which is Equation (13):

[0075]

[0076] Optionally, the process of obtaining the relative pose dynamics equation based on the second-order pose dynamics equation of the target rigid body and the second-order pose dynamics equation of the robotic arm end effector in step S3 includes:

[0077] S301. Define pose tracking error, which is given by formula (14):

[0078] r e =r t -r; (14)

[0079] S302. Calculate the second derivative of the pose tracking error to obtain the relative pose dynamics equation, which is Equation (15):

[0080]

[0081] Optionally, the robust adaptive controller designed for the relative pose dynamics equation in S4 includes:

[0082] S401. Define the sliding surface and obtain the sliding surface formula;

[0083] S402. Differentiate the formula for the sliding surface;

[0084] S403. Define the Lyapunov function and obtain the Lyapunov function formula;

[0085] S404. Differentiate the Lyapunov function formula;

[0086] S405. Design the controller and adaptive law, and obtain the expressions for the controller and adaptive law;

[0087] S406. Based on the controller, the expression of the adaptive law, and the result of differentiating the Lyapunov function formula, the joint torque of the robotic arm required to complete the trajectory tracking control task is obtained.

[0088] Optionally, the formula for the sliding surface in S401 is formula (16):

[0089]

[0090] Among them, Λ=diag{λ1, λ2,…, λ7}, λ i >0, i = 1, 2, ..., 7.

[0091] Optionally, the formula for differentiating 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. The upper bound of the perturbation is estimated as an error. It is an estimate of h; To account for the estimation error of the upper bound of the disturbance torque, yes The estimated values; γ and β are both normal numbers.

[0096] Optionally, the formula for differentiating the Lyapunov function formula in S404 is formula (19);

[0097]

[0098] Optionally, the expression for the controller in S405 is formula (20).

[0099]

[0100] The result of differentiating the expression based on the controller, the adaptive law, and the Lyapunov function formula in S406 is transformed to obtain the joint torque of the robotic arm required to complete the trajectory tracking control task:

[0101] S4061. Combining formula (20) with formula (19), we obtain formula (21):

[0102]

[0103] The adaptive law is given by formula (22):

[0104]

[0105] Where ||s||1 is the first norm of s;

[0106] S4062. Combining formula (22) with formula (21), we get formula (23):

[0107]

[0108] S4063, according to The required joint torque of the robotic arm to complete the trajectory tracking control task is obtained from formula (24):

[0109]

[0110] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following:

[0111] The above scheme studies the relative pose modeling and coordinated control of the robotic arm and the moving target rigid body in three-dimensional space. By establishing a relative pose dynamic model between the feature points of the two rigid bodies, a robust adaptive relative pose tracking control method based on quaternions is proposed. This method can achieve target feature point position tracking and attitude synchronization under conditions where both the moving target rigid body and the robotic arm joints are subject to external disturbances. To address the problem of unknown disturbances affecting the moving target rigid body and the robotic arm joints, this invention introduces an adaptive law to estimate the upper bound of the unknown disturbance and introduces a robust term in the controller to compensate for the impact of external unknown disturbances on the control system. Within the Lyapunov framework, by adjusting the design parameters, it is rigorously proven that the relative position tracking error and attitude synchronization error between the two feature points converge to a small neighborhood of zero. Attached Figure Description

[0112] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0113] Figure 1 This is a flowchart of a robotic arm pose tracking and control method for moving targets provided in an embodiment of the present invention;

[0114] Figure 2 This is a schematic diagram of a scenario in which a robotic arm tracks a moving target rigid body in an embodiment of the present invention;

[0115] Figure 3 It is a graph showing the relative pose of the moving target rigid body and the end effector of the robotic arm over time in an embodiment of the present invention;

[0116] Figure 4 This is a graph showing the derivative of the relative pose of the moving target rigid body and the end effector of the robotic arm as a function of time in an embodiment of the present invention.

[0117] Figure 5 This is a graph showing the change of the joint control torque of the robotic arm over time in an embodiment of the present invention;

[0118] Figure 6 This is a graph showing the change of the estimated upper bound of the unknown disturbance experienced by the target rigid body over time in an embodiment of the present invention.

[0119] Figure 7 This is a graph showing the estimated upper bound of the unknown disturbance torque experienced by the robotic arm in this embodiment of the invention as a function of time. Detailed Implementation

[0120] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the described embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0121] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms “first,” “second,” and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Similarly, the terms “an,” “a,” or “the,” and similar terms do not indicate a quantity limitation, but rather indicate the presence of at least one. The terms “comprising,” “including,” or “including,” and similar terms mean that the element or object preceding the word encompasses the element or object listed following the word and its equivalents, without excluding other elements or objects. The terms “connected,” “linked,” or “connected,” and similar terms are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect.

[0122] It should be noted that the terms "up", "down", "left", "right", "front", and "back" used in this invention are only used to indicate relative positional relationships. When the absolute position of the object being described changes, the relative positional relationship may also change accordingly.

[0123] In control system design, the existence of unknown disturbances is a common and unavoidable problem. Robust adaptive control technology is widely used to address these disturbances. Robust control refers to a class of control methods that can still guarantee system performance and stability even when system parameters are uncertain and external disturbances exist. By designing an adaptive rate, uncertain model parameters and upper bounds of unknown disturbances can be estimated. By introducing robust terms into the controller, these uncertainties can be effectively handled, thereby improving the system's robustness and preventing it from going out of control or malfunctioning due to external disturbances. Through effective handling of unknown disturbances, the system can maintain excellent performance under various operating 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, improving overall reliability. By introducing robust terms and adaptive rates, the stability and reliability of the system are improved, enabling the control system to operate efficiently and stably in various complex and variable environments.

[0124] like Figures 1 to 7As shown, this embodiment provides a robotic arm pose tracking and control method for moving targets. The specific scheme includes:

[0125] S1. Establish the second-order pose dynamics equations of the target rigid body according to the Newton-Euler method;

[0126] S2. Establish the second-order pose dynamics equations of the robotic arm end effector based on the Lagrange equations;

[0127] S3. Based on the second-order pose dynamics equation of the target rigid body and the second-order pose dynamics equation of the robotic arm end effector, the relative pose dynamics equation is obtained.

[0128] S4. Design a robust adaptive controller for the relative pose dynamics equation.

[0129] In one specific implementation, S1 establishes the second-order pose dynamics equation of the target rigid body based on the Newton-Euler method:

[0130] S101, Define the inertial coordinate system O I -X I Y I Z I The fixed coordinate system O of the moving target T -X T Y T Z T and the fixed coordinate system O at the end of the robotic arm E -X E Y E Z E Wherein, the origin of the inertial coordinate system is the base point of the robotic arm base, the origin of the fixed coordinate system of the moving target is the target centroid, and the origin of the fixed coordinate system of the robotic arm end is the robotic arm end.

[0131] S102. Define the position kinematics of the target rigid body as Equation (1):

[0132]

[0133] Where, p t =[p tx p ty p tz ] T For the target rigid body in the inertial coordinate system O I The lower position;

[0134] v t =[v tx v ty v tz ] T For the target rigid body in the inertial coordinate system O IThe speed of the downward movement;

[0135] S103. Define the attitude kinematics of the target rigid body as Equation (2):

[0136]

[0137] in, It is the attitude quaternion of the target rigid body;

[0138] It is q tv 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 For the target rigid body in the inertial coordinate system O I angular velocity at the bottom;

[0141] S104. Based on the position kinematics and attitude kinematics of the target rigid body, the pose kinematics of the target rigid body is obtained, and the pose 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 is given by formula (4):

[0145]

[0146] Where, m t It is the mass of the target rigid body, f t It is the external force acting on the target rigid body;

[0147] S106. Based on the Euler equations of rigid body dynamics, defined in the inertial coordinate system O I The attitude dynamics of the target rigid body are given by formula (5):

[0148]

[0149] Jt It is the moment of inertia of the target rigid body, τ t It is the external torque acting on the target rigid body;

[0150] S107. Differentiate the pose kinematics of the target rigid body, and combine the position dynamics and attitude dynamics of the target rigid body to obtain the second-order pose dynamics equation of the target rigid body, which is Equation (6):

[0151]

[0152] When the target rigid body is subjected to an external force f t and torque τ t All are unknown disturbance signals, but the disturbances have an upper bound and satisfy the following conditions: h is estimated using an adaptive law.

[0153] In one specific implementation, the second-order pose dynamics equation of the robotic arm end effector established by S2 based on the Lagrange equation includes:

[0154] S201. Based on the Lagrange equation, establish the dynamic model of the n-joint robotic arm in the joint space. The dynamic model of the n-joint robotic arm in the joint space is Equation (7):

[0155]

[0156] in, For the joint angles of the robotic arm;

[0157] Joint angular velocity;

[0158] Joint angular acceleration;

[0159] Here is the inertia matrix of the robotic arm;

[0160] The Coriolis force matrix represents the centrifugal force of the robotic arm;

[0161] This is the gravitational torque term of the robotic arm;

[0162] This is the frictional torque term of the robotic arm;

[0163] These are the torques of the various joints of the robotic arm;

[0164] d represents the unknown external disturbance torque, but d has an upper bound, i.e. Estimation via adaptive law;

[0165] S202. When describing the end-effector posture based on Euler angles, the differential kinematics of the end-effector posture is given by formula (8).

[0166]

[0167] in, This represents the position of the robotic arm's end effector in the inertial coordinate system.

[0168] The angular velocity of the robotic arm's end effector in the inertial coordinate system;

[0169] For the Jacobian matrix of the robotic arm;

[0170] For the position components of the Jacobian matrix of the robotic arm;

[0171] For the attitude components of the Jacobian matrix of the robotic arm;

[0172] S203. When using quaternions to describe the end-effector posture, the differential kinematics of the robot arm's end-effector posture is given by formula (10):

[0173]

[0174] in, The quaternion represents the attitude of the robotic arm's end effector in the inertial coordinate system;

[0175] This represents the Jacobian matrix of the robotic arm when the end-effector posture is described using quaternions.

[0176] This represents the attitude components of the Jacobian matrix;

[0177] S204. The kinematic relationship between the derivative of the quaternion of the robotic arm's end-effector posture and the angular velocity satisfies formula (10):

[0178]

[0179] in,

[0180] It is q v skew-symmetric matrix;

[0181] E(q) satisfies E(q) T E(q) = I³, E(q) T q = 0;

[0182] S204. Based on the kinematic relationship between the derivative of the quaternion of the robotic arm's end-effector posture and the angular velocity, and... get And, 2E(q) T J q =J ω ;

[0183] Based on the differential kinematics of the robotic arm's end effector pose, we obtain

[0184] If the robotic arm is not singular, then J A If the rank is full, then J A The row rank is equal to the column rank;

[0185] If the robotic arm is singular, then the corresponding differential inverse kinematics has no solution;

[0186] S205, will and Substituting the dynamic model of the n-joint robotic arm in the joint space, we obtain the dynamic model of the robotic arm end effector in the task space, which is Equation (11):

[0187]

[0188] S206. The dynamic model of the end effector of the robotic arm in the task space is deformed to obtain formula (12):

[0189]

[0190] in,

[0191] S207. Based on the dynamic model of the deformed end effector in the task space, the second-order pose dynamic equation of the end effector is obtained, which is Equation (13):

[0192]

[0193] In one specific implementation, step S3, based on the second-order pose dynamics equation of the target rigid body and the second-order pose dynamics equation of the robotic arm end effector, derives the relative pose dynamics equation, including:

[0194] S301. Define pose tracking error, which is given by formula (14):

[0195] r e =r t -r; (14)

[0196] S302. Calculate the second derivative of the pose tracking error to obtain the relative pose dynamics equation, which is Equation (15):

[0197]

[0198] In one specific implementation, the design of a robust adaptive controller for the relative pose dynamics equation in step S4 includes:

[0199] S401. Define the sliding surface and obtain the 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. Differentiate the sliding surface formula; the formula for differentiating the sliding surface formula is formula (17):

[0203]

[0204] S403. Define the Lyapunov function and obtain the Lyapunov function formula; the Lyapunov function formula is formula (18):

[0205]

[0206] Where V is a positive definite Lyapunov scalar function. The upper bound of the perturbation is estimated as an error. It is an estimate of h; To account for the estimation error of the upper bound of the disturbance torque, yes The estimated values; γ and β are both normal numbers.

[0207] S404. Differentiate the Lyapunov function formula; the formula for differentiating the Lyapunov function formula is formula (19);

[0208]

[0209] S405. Design the controller and adaptive law, and obtain the expressions for the controller and adaptive law;

[0210] The expression for the controller in S405 is formula (20).

[0211]

[0212] S406. Based on the definition of the controller and the result of differentiating the Lyapunov function formula, the required joint torque of the robotic arm to complete the trajectory tracking control task is obtained by transformation, including the following steps:

[0213] S4061. Combining formula (20) with formula (19), we obtain formula (21):

[0214]

[0215] The adaptive law is given by formula (22):

[0216]

[0217] Where ||s||1 is the first norm of s;

[0218] S4062. Combining formula (22) with formula (21), we get formula (23):

[0219]

[0220] S4063, according to The required joint torque of the robotic arm to complete the trajectory tracking control task is obtained from formula (24):

[0221]

[0222] The effectiveness of the robotic arm pose tracking control method for moving targets provided in this embodiment was verified by computer numerical simulation. The simulation platform was based on Matlab software under the Win11 x64 operating system.

[0223] In one specific implementation method, the physical parameters of the moving target rigid body are considered as follows:

[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 disturbance force f acting on the target rigid body t =[ab 0] T (N), external disturbance torque τ t =[00 c] T(N·m), where a, b, and c are random values ​​between -0.00001 and 0.00001.

[0227] Simulation verification was performed using a two-bar linkage robotic arm. 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 Let m1 = 1 (kg), m2 = 0.8 (kg), l1 = 1 (m), and l2 = 0.8 (m). The external disturbance torque acting on the robotic arm is taken as d = [0.0001 0.0001]. T (N·m).

[0231] θ and The initial condition is set to θ(0) = [0 0]. T (rad), At this point, the initial position vector and attitude quaternion of the robotic arm's end effector are p(0) = [1.8 0 0]. T and q(0)=[1 0 0 0] T .

[0232] Initial values ​​for parameter estimation are set as follows: The adjustable parameters are γ = 0.0001, β = 0.00001, Λ = diag{1 1 1 1 1 1 1} and α = 0.12.

[0233] 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 representing the changes in pose tracking error and its derivative are shown, indicating that the relative pose of the moving target rigid body and the end effector of the robotic arm converges to a small neighborhood of zero within 80 s. The steady-state relative pose error and its derivative error are both less than 5 × 10⁻⁶. -4 and 2×10 -4 This means that both translational and rotational motions are well controlled, exhibiting high precision and rapid stability. For a two-link robotic arm, the 3rd, 5th, and 6th elements of the relative pose error and its derivative vector are always 0, therefore... Figure 3 and Figure 4 The simulation results only plotted the curves of the relative pose error and the 1st, 2nd, 4th, and 7th elements of its derivative vector. Figure 5 This indicates the joint output torque of the robotic arm, and the torque converges within 80(s). Furthermore, due to the friction torque term in the robotic arm's dynamics model, there is still a small vibration after the control torque converges. Figure 6 This represents the upper bound estimate of the unknown disturbance experienced by the target rigid body. The curve showing the change. Figure 7 This represents the upper bound estimate of the unknown disturbance torque experienced by the robotic arm. The variation curves were used to demonstrate the effectiveness of the proposed control strategy through simulation.

[0234] The following points need to be explained:

[0235] (1) The accompanying drawings of the embodiments of the present invention only involve the structures involved in the embodiments of the present invention. Other structures can refer to the general design.

[0236] (2) For clarity, the thickness of layers or regions is enlarged or reduced in the drawings used to describe embodiments of the present invention; that is, these drawings are not drawn to actual scale. It is understood that when an element such as a layer, film, region, or substrate is referred to as being “above” or “below” another element, the element may be “directly” located “above” or “below” the other element, or there may be intermediate elements.

[0237] (3) Where there is no conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other to obtain new embodiments.

[0238] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. The scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A robotic arm pose tracking and control method for moving targets, characterized in that, include: S1. Establish the second-order pose dynamics equations of the target rigid body according to the Newton-Euler method; S2. Establish the second-order pose dynamics equations of the robotic arm end effector based on the Lagrange equations; S3. Based on the second-order pose dynamics equation of the target rigid body and the second-order pose dynamics equation of the robotic arm end effector, the relative pose dynamics equation is obtained. S4. Design a robust adaptive controller for the relative pose dynamics equations; include: S401. Define the sliding surface and obtain the sliding surface formula; S402. Differentiate the formula for the sliding surface; S403. Define the Lyapunov function and obtain the Lyapunov function formula; S404. Differentiate the Lyapunov function formula; the formula for differentiating the Lyapunov function formula is formula (19); S405. Design the controller and adaptive law, and obtain the expressions for the controller and adaptive law; S406. Based on the controller, the expression of the adaptive law, and the result of differentiating the Lyapunov function formula, the joint torque of the robotic arm required to complete the trajectory tracking control task is obtained by transformation. The expression for the controller in S405 is formula (20). The result of differentiating the expression based on the controller, the adaptive law, and the Lyapunov function formula in S406 is transformed to obtain the joint torque of the robotic arm required to complete the trajectory tracking control task: S4061. Combining formula (20) with formula (19), we obtain formula (21): The adaptive law is given by formula (22): Where ||s||1 is the first norm of s; S4062. Combining formula (22) with formula (21), we obtain formula (23): S4063, according to The required joint torque of the robotic arm to complete the trajectory tracking control task is obtained from formula (24):

2. The robotic arm pose tracking and control method for moving targets according to claim 1, characterized in that, The second-order pose dynamics equation of the target rigid body is established according to the Newton-Euler method in S1: S101, Define the inertial coordinate system O I -X I Y I Z I The fixed coordinate system O of the moving target T -X T Y T Z T and the fixed coordinate system O at the end of the robotic arm E -X E Y E Z E Wherein, the origin of the inertial coordinate system is the base point of the robotic arm base, the origin of the fixed coordinate system of the moving target is the target centroid, and the origin of the fixed coordinate system of the robotic arm end is the robotic arm end. S102. Define the position kinematics of the target rigid body as Equation (1): Where, p t =[p tx p ty p tz ] T For the target rigid body in the inertial coordinate system O I The lower position; v t =[v tx v ty v tz ] T For the target rigid body in the inertial coordinate system O I The speed of the downward movement; S103. Define the attitude kinematics of the target rigid body as Equation (2): in, It is the attitude quaternion of the target rigid body; It is q tv 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 For the target rigid body in the inertial coordinate system O I angular velocity at the bottom; S104. Based on the position kinematics and attitude kinematics of the target rigid body, the pose kinematics of the target rigid body is obtained, and the pose 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 is given by formula (4): Where, m t It is the mass of the target rigid body, f t It is the external force acting on the target rigid body; S106. Based on the Euler equations of rigid body dynamics, defined in the inertial coordinate system O I The attitude dynamics of the target rigid body are given by formula (5): J t It is the moment of inertia of the target rigid body, τ t It is the external torque acting on the target rigid body; S107. Differentiate the pose kinematics of the target rigid body, and combine the position dynamics and attitude dynamics of the target rigid body to obtain the second-order pose dynamics equation of the target rigid body, which is Equation (6): When the target rigid body is subjected to an external force f t and torque τ t All are unknown disturbance signals, but the disturbances have an upper bound and satisfy the following conditions: h is estimated using an adaptive law.

3. The robotic arm pose tracking and control method for moving targets according to claim 2, characterized in that, The second-order pose dynamics equations of the robotic arm end effector established by S2 based on the Lagrange equations include: S201. Based on the Lagrange equation, establish the dynamic model of the n-joint robotic arm in the joint space. The dynamic model of the n-joint robotic arm in the joint space is Equation (7): in, For the joint angles of the robotic arm; Joint angular velocity; Joint angular acceleration; Here is the inertia matrix of the robotic arm; The Coriolis force matrix represents the centrifugal force of the robotic arm; This is the gravitational torque term of the robotic arm; This is the frictional torque term of the robotic arm; These are the torques of the various joints of the robotic arm; d represents the unknown external disturbance torque, but d has an upper bound, i.e. Estimation via adaptive law; S202. When describing the end-effector posture based on Euler angles, the differential kinematics of the end-effector posture is given by formula (8). in, This represents the position of the robotic arm's end effector in the inertial coordinate system. The angular velocity of the robotic arm's end effector in the inertial coordinate system; For the Jacobian matrix of the robotic arm; For the position components of the Jacobian matrix of the robotic arm; For the attitude components of the Jacobian matrix of the robotic arm; S203. When using quaternions to describe the end-effector posture, the differential kinematics of the robot arm's end-effector posture is given by formula (10): in, The quaternion represents the attitude of the robotic arm's end effector in the inertial coordinate system; This represents the Jacobian matrix of the robotic arm when the end-effector posture is described using quaternions. This represents the attitude components of the Jacobian matrix; S204. The kinematic relationship between the derivative of the quaternion of the robotic arm's end-effector posture and the angular velocity satisfies formula (10): in, It is q v skew-symmetric matrix; E(q) satisfies E(q) T E(q) = I³, E(q) T q = 0; S204. Based on the kinematic relationship between the derivative of the quaternion of the robotic arm's end-effector posture and the angular velocity, and... get And, 2E(q) T J q =J ω ; Based on the differential kinematics of the robotic arm's end effector pose, we obtain If the robotic arm is not singular, then J A If the rank is full, then J A The row rank is equal to the column rank; If the robotic arm is singular, then the corresponding differential inverse kinematics has no solution; S205, will and Substituting the dynamic model of the n-joint robotic arm in the joint space, we obtain the dynamic model of the robotic arm end effector in the task space, which is Equation (11): S206. The dynamic model of the end effector of the robotic arm in the task space is deformed to obtain formula (12): in, S207. Based on the dynamic model of the deformed end effector in the task space, the second-order pose dynamic equation of the end effector is obtained, which is Equation (13):

4. The robotic arm pose tracking and control method for moving targets according to claim 3, characterized in that, The relative pose dynamics equation obtained by S3 based on the second-order pose dynamics equation of the target rigid body and the second-order pose dynamics equation of the robotic arm end effector includes: S301. Define pose tracking error, which is given by formula (14): r e ＝r t -r；(14) S302. Calculate the second derivative of the pose tracking error to obtain the relative pose dynamics equation, which is Equation (15):

5. The robotic arm pose tracking and control method for moving targets according to claim 4, characterized in that, The formula for the sliding surface in S401 is formula (16): Where, Λ=diag{λ1,λ2,…,λ7}, λ i >0, i=1,2,…,7.

6. The robotic arm pose tracking and control method for moving targets according to claim 5, characterized in that, The formula for differentiating the sliding surface formula in S402 is formula (17):

7. The robotic arm pose tracking and control method for moving targets according to claim 6, characterized in that, The Lyapunov function formula in S403 is formula (18): Where V is a positive definite Lyapunov scalar function. The upper bound of the perturbation is estimated as an error. It is an estimate of h; To account for the estimation error of the upper bound of the disturbance torque, yes The estimated values; γ and β are both normal numbers.

Images