A battery replacing robot mechanical arm tracking control method and device, storage medium and electronic equipment

Abstract

The application discloses a battery replacing robot mechanical arm tracking control method and device, a storage medium and electronic equipment, and relates to the technical field of battery replacing robots. The method comprises the following steps: establishing a battery replacing robot trajectory tracking control model; designing a sliding mode function and a sliding mode reaching law based on the tracking error of the mechanical arm; bringing the designed sliding mode function and sliding mode reaching law into the battery replacing robot mechanical arm trajectory tracking control model to obtain a battery replacing robot mechanical arm trajectory tracking control rate; and controlling the battery replacing robot mechanical arm by using the trajectory tracking control rate. The application proposes a novel sliding mode reaching law, which can improve the convergence speed while reducing system chattering, and gives the convergence time. The method can further improve the dynamic performance and convergence speed of the battery replacing robot mechanical arm, and has important significance for improving work efficiency and saving labor.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, specifically relating to a method, device, storage medium, and electronic equipment for tracking and controlling the robotic arm of a battery swapping robot. Background Technology

[0002] Currently, there are numerous types of passenger vehicles and battery packs requiring battery swapping, resulting in long swapping times. Battery swapping stations cannot provide one-stop, fast swapping for multiple vehicle models and battery pack types, failing to meet the requirements of users and operators. Therefore, achieving shared battery swapping for multiple brands, models, and battery pack models is a common challenge. A universal battery swapping robot's robotic arm is equipped with a battery pack locking / unlocking device at its end. Using image information from a vision sensor, it quickly locates the coordinates of the battery pack latches, enabling precise positioning, rapid planning, path tracking control, and automatic locking / unlocking. This effectively solves the problem of compatibility issues caused by varying battery pack sizes.

[0003] The general-purpose battery-swapping robotic arm is a multi-input multi-output system with undesirable characteristics such as high nonlinearity, strong dynamic coupling, parameter perturbation, and unknown disturbances. Tracking control of the robotic arm has been a key research focus and challenge in recent years. However, current trajectory tracking control for robotic arms still faces complex problems, and sliding mode control is one of the main methods for solving these problems. However, the tracking control of battery-swapping robots still faces the following difficulties: First, chattering is unavoidable and can only be mitigated to a certain extent, which becomes a prominent obstacle to the application of sliding mode variable structure control in practical systems. Second, general sliding mode controllers can only guarantee asymptotic stability, but asymptotic stability means that the closed-loop system state only converges to the equilibrium point when time approaches infinity, which cannot well meet the requirements of fast stability in practical engineering. Third, the finite-time convergence time depends on the initial value of the closed-loop system state, and the convergence time changes with the initial state. Currently used trajectory tracking control methods for battery-swapping robots do not have suitable sliding mode reaching laws designed to reduce chattering and shorten the system response time. These difficulties make the trajectory tracking control of battery-swapping robotic arms a highly challenging research topic. Summary of the Invention

[0004] The purpose of this invention is to provide a tracking control method, device, storage medium, and electronic device for a battery swapping robot arm. Based on a novel approach law, a sliding mode controller is designed to track and control the battery swapping robot arm, which can further improve the dynamic performance and convergence speed of the arm.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows:

[0006] This invention provides a method for tracking and controlling the robotic arm of a battery swapping robot, comprising:

[0007] Establish a trajectory tracking and control model for the robotic arm of the battery swapping robot;

[0008] Design of sliding mode function and sliding mode reaching law based on tracking error of robotic arm;

[0009] Substituting the sliding mode function and sliding mode approach law into the trajectory tracking control model of the battery swapping robot arm, the trajectory tracking control law of the battery swapping robot arm is obtained.

[0010] The trajectory tracking control law is used to control the robotic arm of the battery swapping robot.

[0011] Furthermore, the establishment of the trajectory tracking control model for the battery swapping robot arm includes:

[0012] Establish a dynamic model for the two-joint robotic arm of the battery swapping robot:

[0013]

[0014] in, Let q represent the position, angular velocity, and angular acceleration of the robotic arm, respectively. For a two-joint robotic arm, q = [q1, q2]. T q1 and q2 are the current angles of joint 1 and joint 2 of the robotic arm, respectively, and M(q) is the inertia matrix of the robotic arm. Let G(q) be the matrix of centrifugal force and Coriolis force, and G(q) be the gravity vector of the robotic arm. Let τ be the matrix of frictional forces. d The external perturbation matrix satisfies |τ d |≤d, where d is the upper bound of the applied disturbance, and τ is the trajectory tracking control law of the robotic arm.

[0015] Furthermore, the design of the sliding mode function and sliding mode reaching law based on the tracking error of the robotic arm includes:

[0016] The sliding mode function is designed based on the tracking error of the robotic arm as follows:

[0017]

[0018] Where σ is the sliding mode function and e is the tracking error. p > r > 0 are positive odd numbers, κ1 > 0, κ2 > 0 are constants;

[0019] Tracking error is defined as:

[0020] e = q d -q;

[0021]

[0022]

[0023] in, The first derivative of the tracking error, q is the second derivative of the tracking error. d For the desired trajectory of the robotic arm,

[0024] For a two-joint robotic arm, there exists e = [e1, e2]. T q d =[q d1 ,q d2 ] T e1 and e2 are the tracking errors of joints 1 and 2 of the robotic arm, respectively, and q d1 ,q d2 These are the expected trajectories of robotic arm joints 1 and 2, respectively.

[0025] The sliding mode convergence law is designed as follows:

[0026]

[0027] in, Let S be the sliding mode reaching law, S be the sliding surface, and α, η, γ, k, and ν be sliding mode parameters, satisfying: 0 < α < 1, η + γ < 1, μ = ν|σ(0)| 1-α σ(0) is the initial value of the sliding mode function, and k,ν are positive numbers;

[0028] For a two-joint robotic arm, the following exists:

[0029] S = [σ1, σ2] T ;

[0030]

[0031]

[0032] σ1 and σ2 are the sliding mode functions of joint 1 and joint 2 of the robotic arm, respectively.

[0033] Furthermore, by substituting the sliding mode function and sliding mode convergence law into the trajectory tracking control model of the battery swapping robot arm, the trajectory tracking control law of the battery swapping robot arm is obtained, including:

[0034] Taking the first derivative of the sliding surface, we get:

[0035]

[0036] Where diag{·} denotes a diagonal matrix;

[0037] Substituting the second derivative of the tracking error, the first derivative of the sliding surface, and the sliding mode reaching law into the trajectory tracking control model of the battery swapping robot arm, we obtain:

[0038]

[0039] Design robustness term τ r =Dsign(S), replacing the external disturbance matrix τ in the trajectory tracking control model of the battery swapping robot arm. d The trajectory tracking control rate of the battery swapping robot's robotic arm was obtained:

[0040]

[0041] Furthermore, the method also includes verifying the finite-time convergence of the trajectory tracking control law using a Lyapunov function.

[0042] The Lyapunov function is selected as follows:

[0043]

[0044] A second aspect of the present invention provides a tracking and control device for a battery swapping robot arm, used to implement the aforementioned battery swapping robot arm control method, the device comprising:

[0045] The model building module is used to establish a trajectory tracking and control model for the battery swapping robot's robotic arm.

[0046] The parameter design module is used to design sliding mode functions and sliding mode reaching laws based on the tracking error of the robotic arm;

[0047] The calculation module is used to input the sliding mode function and sliding mode approach law into the trajectory tracking control model of the battery swapping robot arm to obtain the trajectory tracking control rate of the battery swapping robot arm.

[0048] The control module is used to control the robotic arm of the battery swapping robot using the trajectory tracking control law.

[0049] A third aspect of the present invention provides a computer-readable storage medium for storing one or more programs, the one or more programs including instructions that, when executed by a computing device, cause the computing device to perform any of the methods described above.

[0050] A fourth aspect of the present invention provides an electronic device, comprising,

[0051] One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including instructions for performing any of the methods described above.

[0052] The beneficial effects of this invention are:

[0053] (1) This invention provides a tracking control method for the robotic arm of a battery swapping robot, and designs a novel sliding mode approach law that can improve the convergence speed while reducing system chattering.

[0054] (2) The sliding mode controller designed based on the novel sliding mode convergence law of the present invention has control continuity and further reduces the chattering problem compared with the traditional sliding mode controller; at the same time, it can converge stably within a finite time and has higher tracking accuracy. Attached Figure Description

[0055] Figure 1 A flowchart of a battery swapping robot robotic arm tracking control method provided by the present invention;

[0056] Figure 2 The diagram shows the position tracking control of a two-joint robotic arm under sliding mode control according to three convergence laws in the embodiments of the present invention.

[0057] Figure 3 This is a speed tracking control diagram of a two-joint robotic arm under sliding mode control, given in an embodiment of the present invention.

[0058] Figure 4 The inputs are for the two-joint robotic arm control under sliding mode control, as given in the embodiments of the present invention, based on the three convergence laws. Detailed Implementation

[0059] The present invention will be further described below. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention.

[0060] This invention provides a tracking control method for the robotic arm of a battery swapping robot, see [link to relevant documentation]. Figure 1 ,include:

[0061] Establish a trajectory tracking and control model for the robotic arm of the battery swapping robot;

[0062] Design of sliding mode function and sliding mode reaching law based on tracking error of robotic arm;

[0063] The designed sliding mode function and sliding mode approach law are substituted into the established trajectory tracking control model of the battery swapping robot arm to obtain the trajectory tracking control law of the battery swapping robot arm.

[0064] The obtained trajectory tracking control law is used to control the robotic arm of the battery swapping robot.

[0065] In one embodiment of the present invention, a trajectory tracking control model for the battery swapping robot arm is established, and the specific process is as follows:

[0066] A dynamic model of the two-joint robotic arm of the battery swapping robot is established using dynamic methods such as the Lagrange method:

[0067]

[0068] In the formula, Let q represent the position, angular velocity, and angular acceleration of the robotic arm, respectively. For a two-joint robotic arm, q = [q1, q2]. T q1 and q2 are the current angles of the two joints, and M(q) is the inertia matrix of the robotic arm. Let G(q) be the matrix of centrifugal force and Coriolis force, and G(q) be the gravity vector of the robotic arm. Let τ be the matrix of frictional forces. d Let be the matrix formed by the external perturbation. Generally, it is assumed that the external perturbation has an upper bound, i.e.: |τ d |≤d, τ is the trajectory tracking control law of the robotic arm.

[0069] For a two-degree-of-freedom robotic arm, neglecting the joint mass, the masses of the two links are m1 and m2, and the lengths of the links are l1 and l2, respectively. q1 and q2 are the current angles of the two joints.

[0070] The parameter matrix of the two-degree-of-freedom robotic arm is:

[0071]

[0072] in,

[0073]

[0074]

[0075]

[0076] ξ1 and ξ2 are the friction coefficients of joint 1 and joint 2 of the robotic arm, respectively.

[0077] In one embodiment of the present invention, a sliding mode function and a sliding mode reaching law are designed based on the tracking error of the robotic arm. The specific implementation process is as follows:

[0078] The sliding mode function is designed as follows:

[0079]

[0080] Where σ is the sliding mode function, p > r > 0 represents positive odd numbers, κ1 > 0 and κ2 > 0 represent constants, and e represents the tracking error.

[0081] Tracking error is defined as:

[0082] e = q d -q = [e1, e2] T(7)

[0083]

[0084]

[0085] Here, e represents the tracking error. The first derivative of the tracking error, q is the second derivative of the tracking error. d For the desired trajectory of the robotic arm,

[0086] For a two-joint robotic arm, there exists e = [e1, e2]. T q d =[q d1 ,q d2 ] T e1 and e2 are the tracking errors of joints 1 and 2 of the robotic arm, respectively, and q d1 ,q d2 These are the expected trajectories of robotic arm joints 1 and 2, respectively.

[0087] The sliding surface is designed as follows:

[0088]

[0089] Where S is the sliding surface.

[0090]

[0091] The first derivative of S is:

[0092]

[0093] Where diag{·} denotes a diagonal matrix;

[0094] The sliding mode convergence law based on the sliding surface design is as follows:

[0095]

[0096] Among them, 0<α<1, η+γ<1, μ=ν|σ(0)| 1-α σ(0) is the initial value of the sliding mode function, and k,ν are positive numbers.

[0097] Furthermore, the finite-time convergence verification of the designed sliding mode reaching law is as follows:

[0098] Integrating the designed sliding mode reaching law, we obtain:

[0099]

[0100] The time t is when the sliding mode function σ reaches the sliding surface.r For equation (13) from 0 to t r The integral yields:

[0101]

[0102] The solution to the second definite integral is as follows:

[0103]

[0104] The approach time can be obtained as follows:

[0105]

[0106] It is evident that the designed convergence law can be reached within a finite amount of time.

[0107] In one embodiment of the present invention, the designed sliding mode function and sliding mode convergence law are substituted into the established trajectory tracking control model of the battery swapping robot to obtain the trajectory tracking control rate of the battery swapping robot's robotic arm, as follows:

[0108] Substituting the second derivative of the tracking error (9), the first derivative of the sliding surface (11), and the sliding mode approach law (12) into the trajectory tracking control model (1) of the battery swapping robot arm, we obtain:

[0109]

[0110] Design robustness term τ r =Dsign(S) replaces the external perturbation matrix τ d Where D > d, and D represents the upper bound of the disturbance.

[0111] The control law for tracking the trajectory of the battery swapping robot arm is obtained as follows:

[0112]

[0113] The finite-time convergence property of the above tracking control law is verified as follows:

[0114] Choose the Lyapunov function as:

[0115]

[0116] Taking its derivative, we get:

[0117]

[0118] From equation (11), we can calculate:

[0119]

[0120] Substituting equations (18) and (1) into the above equation, we can obtain:

[0121]

[0122] Because external disturbances have an upper bound |τ d |≤d, and D≥d. Therefore, -Dsign(S)+τ d ≤0. Therefore:

[0123]

[0124] here,

[0125] λ max This represents the largest eigenvalue of M(q).

[0126] Clearly, the designed control law (18) can converge to zero within a finite time T1.

[0127] When the system is in sliding mode motion, σ1 = 0, then:

[0128]

[0129] Differentiating equation (24), we get:

[0130]

[0131] As can be seen from the above, equation (25) is a global finite-time convergence to the equilibrium point.

[0132] In one embodiment of the present invention, a novel sliding mode controller based on the approach law proposed in this invention is used to track and control the robotic arm of a battery swapping robot, and the results are compared with traditional sliding mode controllers based on exponential and power approaches.

[0133] The parameters of the robotic arm model are: m1 = 50, m2 = 125, l1 = 1.2, l2 = 2.0, ξ1 = ξ2 = 12, g = 9.81. The parameters of the sliding mode controller are: α = 0.6, γ = 0.4, η = 0.4, ν = 1, p = 9, r = 7, κ1 = 3, κ2 = 1, k = 2.

[0134] Based on the power-law approach law, the exponential-law approach law, and the novel approach law proposed in this invention, sliding mode controllers for two-degree-of-freedom robotic arms are designed respectively. The applied external disturbance is set as τ. d =[2sin(2πt); 3cos(2πt)].

[0135] Figure 2 and Figure 3The position and velocity tracking trajectories of the battery swapping robot's robotic arm were tracked using sliding mode controllers designed with three different reaching laws. It can be seen that the root mean square error (RMSE) of the position tracking using the novel reaching law proposed in this invention is [0.0571, 1.4328], while the RMSEs of the exponential and power reaching laws are [0.0738, 1.4208] and [0.0635, 1.4288], respectively. Therefore, the sliding mode tracking control method based on the novel reaching law proposed in this invention has higher tracking accuracy.

[0136] Figure 4 To control the input, it can be seen that the sliding mode approach law designed in the invention can reduce chattering.

[0137] A second aspect of the present invention also provides a tracking and control device for a battery swapping robot arm, used to implement the above-described battery swapping robot arm control method, the device comprising:

[0138] The model building module is used to establish a trajectory tracking and control model for the battery swapping robot's robotic arm.

[0139] The parameter design module is used to design sliding mode functions and sliding mode reaching laws based on the tracking error of the robotic arm;

[0140] The calculation module is used to input the sliding mode function and sliding mode approach law into the trajectory tracking control model of the battery swapping robot arm to obtain the trajectory tracking control rate of the battery swapping robot arm.

[0141] The control module is used to control the robotic arm of the battery swapping robot using the trajectory tracking control law.

[0142] It is worth noting that this device embodiment corresponds to the above method embodiment. The implementation methods of the above method embodiments are all applicable to this device embodiment and can achieve the same or similar technical effects, so they will not be described in detail here.

[0143] A third aspect of the present invention also provides a computer-readable storage medium for storing one or more programs, the one or more programs including instructions that, when executed by a computing device, cause the computing device to perform the above-described battery swapping robot robotic arm control method.

[0144] A fourth aspect of the present invention also provides an electronic device, comprising,

[0145] One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for performing the above-described battery swapping robot robotic arm control method.

[0146] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0147] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0148] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0149] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0150] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A battery swapping robot manipulator tracking control method, characterized in that, The method comprises: a trajectory tracking control model of the battery swap robot mechanical arm is established; a sliding mode function and a sliding mode reaching law are designed based on a tracking error of the mechanical arm, comprising: the sliding mode function is designed based on the tracking error of the mechanical arm as follows: ； wherein, is a sliding mode function, is a tracking error, , is a positive odd integer, is a constant; the tracking error is defined as: ； ； ； wherein, is a first derivative of the tracking error, is a second derivative of the tracking error, is a desired trajectory of the robot arm, are position, angular velocity and angular acceleration of the robot arm, respectively; For a two-joint robot arm, there exist , , are the tracking errors for the robot arm joints 1 and 2, respectively, are the desired trajectories for the robot arm joints 1 and 2, respectively; the sliding mode reaching law is designed as: ； wherein, is a sliding mode reaching law, is a sliding mode surface, , , , and are sliding mode parameters satisfying: , , , is an initial value of the sliding mode function, is a positive number; for a two-joint mechanical arm, there are: ； ， ； ， ； , These are the sliding mode functions for robotic arm joints 1 and 2, respectively. They are respectively The first derivative, the sliding mode function and the sliding mode reaching law are brought into the trajectory tracking control model of the battery swap robot mechanical arm to obtain a trajectory tracking control rate of the battery swap robot mechanical arm; the trajectory tracking control rate is used to control the battery swap robot mechanical arm.

2. The battery swapping robot mechanical arm tracking control method according to claim 1, characterized in that, The trajectory tracking control model of the battery swap robot mechanical arm is established, comprising: a dynamics model of a two-joint mechanical arm of the battery swap robot is established: ； where, are the position, angular velocity and angular acceleration of the manipulator, respectively, and , are the current angles of the joints 1 and 2 of the manipulator, respectively, is the inertia matrix of the manipulator, is the centrifugal and Coriolis force matrix, is the gravity vector of the manipulator, is the matrix of friction forces, is the matrix of external disturbances, satisfying , is the upper bound of the external disturbances, is the trajectory tracking control law of the manipulator.

3. The battery swapping robot mechanical arm tracking control method according to claim 2, characterized in that, the sliding mode function and the sliding mode reaching law are brought into the trajectory tracking control model of the battery swap robot mechanical arm to obtain a trajectory tracking control rate of the battery swap robot mechanical arm, comprising: a first-order derivative of the sliding surface is obtained as follows: ； wherein denotes a diagonal matrix; the second-order derivative of the tracking error, the first-order derivative of the sliding surface, and the sliding mode reaching law are substituted into the trajectory tracking control model of the battery swap robot mechanical arm to obtain: ； Robust term , instead of the added disturbance matrix in the trajectory tracking control model of the battery swap robot mechanical arm , get the trajectory tracking control rate of the battery swap robot mechanical arm: 。 4. The battery swapping robot mechanical arm tracking control method according to claim 3, characterized in that, The method further comprises verifying the finite time convergence of the trajectory tracking control rate using a Lyapunov function, the Lyapunov function is selected as: 。 5. A battery swapping robot mechanical arm tracking control device, characterized in that, The device is used to implement the battery swap robot mechanical arm tracking control method according to any one of claims 1 to 4, and the device comprises: a model construction module configured to establish a trajectory tracking control model of a battery swap robot mechanical arm; a parameter design module configured to design a sliding mode function and a sliding mode reaching law based on a tracking error of the mechanical arm; a calculation module configured to bring the sliding mode function and the sliding mode reaching law into the trajectory tracking control model of the battery swap robot mechanical arm to obtain a trajectory tracking control rate of the battery swap robot mechanical arm; a control module configured to use the trajectory tracking control rate to control the battery swap robot mechanical arm.

6. A computer-readable storage medium storing one or more programs, the one or more programs comprising instructions that when executed by a computer cause the computer to perform a method comprising: The one or more programs include instructions that, when executed by a computing device, cause the computing device to perform any of the methods of claims 1 to 4.

7. An electronic device, comprising: The one or more processors, the memory, and the one or more programs are included, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for performing any of the methods of claims 1 to 4. ​

Images