Dynamic Sliding Mode Motion Control Method for Mobile Manipulator Based on Online Identification Feedforward of Dynamics

Abstract

The present invention belongs to the field of robot motion control, and discloses a dynamic sliding mode motion control method for a mobile manipulator based on online identification feedforward of dynamics, including Step 1: designing a physically consistent linearized coupled dynamic model of the mobile manipulator; Step 2: designing an adaptive Kalman filtering algorithm based on dynamic update of the modeling error covariance to achieve high-precision online identification of the physically consistent dynamic parameters of the mobile manipulator; Step 3: designing a recursive dynamic feedforward algorithm for the mobile manipulator; Step 4: designing an adaptive super-twisting dynamic sliding mode control algorithm based on the recursive dynamic feedforward to ensure the continuity of the control law and achieve high-precision finite-time convergence of the motion error of the mobile manipulator; Step 5: using the Lyapunov design method to achieve the stability of the controller, forming a paradigm framework for finite-time motion control of robot dynamics feedforward, which can effectively improve the response speed and accuracy of robot motion control.

Description

Technical Field

[0001] The present invention belongs to the field of robot motion control, and particularly relates to a dynamic sliding mode motion control method for a mobile manipulator based on online identification feedforward of dynamics. Background Art

[0002] A mobile manipulator composed of a mobile platform and a manipulator has a vast motion space and flexible operation forms, and is gradually being applied to industrial scenarios such as spraying, inspection, loading and unloading, and service fields such as picking, door opening, and collaborative handling. However, the motion control performance of the mobile manipulator is still restricted by the complex system dynamics coupling and its uncertainty. For example, the entrainment velocity of the mobile platform motion causes centrifugal / Coriolis force effects on the manipulator, and at the same time, the motion of the manipulator will also cause changes in the inertial force and centrifugal / Coriolis force of the mobile platform. In addition, when the mobile manipulator processes unknown loads or faces uneven roads in a complex environment, it will also cause changes in its dynamic properties such as mass, center of gravity, and inertia tensor. Therefore, constructing a vehicle-arm coupling dynamics model for the mobile manipulator and developing an advanced motion control algorithm to address the above problems are the keys to its efficient and high-quality operation.

[0003] Currently, most robot motion controllers require a dynamics calculation model to compensate for the influence of time-varying non-linear strong coupling dynamics characteristics on motion control performance. Due to manufacturing errors and CAD modeling errors, the robot dynamics parameters derived by methods such as CAD / CAM are inaccurate and rarely directly used in controller design. Based on the input and output of the robot system, there are currently various numerical calculation methods such as the least squares method and the particle swarm algorithm used to identify the dynamics model of the robot. However, most of these methods can only achieve offline identification of dynamics parameters and are difficult to be directly used in the mobile manipulator system with real-time changes in dynamics characteristics caused by complex tasks and unstructured environments. In addition, although the recursive least squares method, adaptive neural network, adaptive observer, etc. can achieve online estimation of robot system parameters, these online identification methods cannot meet the physical property constraints of parameters such as mass and inertia, that is, the obtained dynamics parameters do not have physical consistency, so it is still difficult to achieve high-precision dynamic feedforward control of the mobile manipulator. To address problems such as robot dynamics uncertainty and external disturbances, existing motion control methods such as neural network control, sliding mode control, and observer compensation control only consider the bounded convergence of motion errors, and rarely optimize the design of the convergence speed. Moreover, the controller stability design depends on prior knowledge such as the supremum of system uncertainty that is difficult to directly obtain, as well as the complex calculation of the dynamics of redundant robot systems such as mobile manipulators, which limits the practical application of the above control methods. Summary of the Invention

[0004] The object of the present invention is to provide a dynamic sliding mode motion control method for a mobile manipulator based on online identification feedforward of dynamics, so as to solve the above technical problems.

[0005] To solve the above technical problems, the specific technical solution of the dynamic sliding mode motion control method for a mobile manipulator based on online identification feedforward of dynamics of the present invention is as follows:

[0006] The dynamic sliding mode motion control method for a mobile manipulator based on online identification feedforward of dynamics includes the following steps:

[0007] Step 1: Design a vehicle-arm coupled linearized dynamics model of the mobile manipulator based on the recursive Newton-Euler algorithm, and transform the physical consistency constraints of the inertia parameters to form a linearized modeling method that satisfies the physical consistency constraints of the robot dynamics parameters;

[0008] Step 2: Design an adaptive Kalman filter algorithm based on dynamic real-time update of the system modeling error covariance to achieve high-precision online identification of the physical consistency dynamics parameters of the mobile manipulator;

[0009] Step 3: Design a recursive dynamics feedforward algorithm for the mobile manipulator based on the online identification results of the physical consistency dynamics parameters;

[0010] Step 4: Design an adaptive dynamic sliding mode motion control algorithm for the mobile manipulator based on recursive dynamics feedforward, and an adaptive algorithm for the gain parameters of the super-twisting dynamic sliding mode reaching law to achieve high-precision finite-time convergence of the motion error of the mobile manipulator;

[0011] Step 5: Use the Lyapunov design method to achieve the finite-time stability of the entire closed-loop system of the dynamics online feedforward - adaptive dynamic sliding mode control, and form a paradigm framework for the finite-time motion control of the robot dynamics feedforward.

[0012] Furthermore, the specific steps of the above Step 1 include the following:

[0013] Establish the driving force and torque equations required for the motion of the i-th link of the manipulator based on the recursive Newton-Euler algorithm, as follows

[0014]

[0015] In formula (1), i iω i and respectively represent the angular velocity and angular acceleration of the i-th link, represents the linear acceleration of the i-th link; is the regression matrix of the dynamics equation, is the dynamic parameter vector of the i-th link, including the link mass m i , the centroid moment c i = [c ix , c iy , c iz T and the link inertia parameter vector relative to the link coordinate system where the inertia matrix operator and are defined as:

[0016]

[0017] Establish the linearized dynamic model of the mobile platform as follows:

[0018]

[0019] In formula (3) represents the equivalent driving force and moment of the mobile platform, and represent the translational velocities of the mobile platform along the O w X w Y w Z w axis and the rotational velocity about the O w X w axis, O w Y w axis in the world coordinate system O w Z w axis; represents the regression matrix of the mobile platform dynamic equation, Φ p = [m p , l p T is its inertia parameter vector, including the mass m p and the moment of inertia l p ;

[0020] Based on this, construct the vehicle-arm coupled linearized dynamic model of the mobile manipulator as follows:

[0021]

[0022] In formula (4) and q = w x p , w y p , w φ p , θ1, …, θ n T ​​​respectively represent the generalized joint space driving torque and joint position of the mobile manipulator, where θ i , i = 1, …, n are the joint positions of the manipulator; where the force / torque transfer matrix and respectively represent the pose rotation transformation matrix and origin translation vector of two adjacent links; S pp = a p , where i ix i = [1, 0, 0] T , i y i = [0, 1, 0] T , i z i = [0, 0, 1] T , the force / torque transfer matrix and represent the regression matrix of the linearized dynamic model of the mobile manipulator, is the vector of unknown dynamic parameters of the mobile manipulator;

[0023] The dynamic parameters of the mobile manipulator need to satisfy the physical consistency constraints, that is, the mass of each link is greater than zero, the inertia tensor matrix is symmetric positive definite and satisfies the triangle inequality constraint; define the parameters to be identified of the i-th link of the manipulator as Ψ i = [ψ im , ψ ic , ψ il1 , ψ il2 , ψ il3 , ψ il4 , ψ il5 , ψ il6 T , then its dynamic parameters Φ i satisfying the physical consistency constraints are encoded as

[0024]

[0025] ψ in formula (5) ic = [ψ icx , ψ icy , ψ icz T represents the mass moment, ε > 0 is a custom small constant, represents a lower triangular matrix; similarly, define the parameters to be identified of the mobile platform as Ψ p = [ψ pm ​​, ψ pl T , then its dynamic parameter Φ that satisfies the physical consistency constraint p is encoded as:

[0026]

[0027] Substituting formulas (5) and (6) into formula (4), the physical consistency linearized dynamic model of the mobile robot can be obtained as follows:

[0028]

[0029] In formula (7) represents the parameter to be identified, is the dynamic parameter of the mobile manipulator reconstructed to satisfy the physical consistency constraint.

[0030] Furthermore, step 2 includes the following specific steps:

[0031] Step 2.1: Real-time update of dynamic parameters;

[0032] Step 2.2: Measurement correction of dynamic parameters.

[0033] Furthermore, the real-time update of the dynamic parameters in step 2.1 is as follows:

[0034]

[0035] Furthermore, the measurement correction of the dynamic parameters in step 2.2 is as follows:

[0036]

[0037] In formulas (8) and (9) and represent the estimated value of the parameter vector to be identified and the estimated value of its modeling error covariance matrix, Q Φ and R Φ represent the covariance matrices of the system modeling error and the measurement error respectively; is the Kalman filter gain, is the corrected modeling error covariance matrix; is the corrected value of the parameter vector to be identified, and the physical consistency dynamic parameters of the mobile manipulator are obtained

[0038] Design the dynamic real-time update law of the system modeling error covariance as follows:

[0039]

[0040] ​And design the estimated value of the system measurement noise covariance matrix within N sampling periods as follows:

[0041]

[0042] Thus, high-precision online identification of the physical consistency dynamic parameters of the mobile manipulator is achieved.

[0043] Furthermore, step 3 includes the following specific steps:

[0044] Step 3.1: Forward recursively calculate the driving force / torque required for the independent movement of each link;

[0045]

[0046] Step 3.2: Backward recursively calculate the driving force / torque required for the coupled movement of each link:

[0047]

[0048] Step 3.3: Calculate the dynamic feedforward force / torque values of each joint of the mobile manipulator:

[0049]

[0050] In formulas (12)-(14) i ω id 、 and represent the desired link velocity and acceleration values calculated from the desired trajectory of the mobile manipulator's generalized joint space Thus, the dynamic feedforward torque vector of the mobile manipulator is

[0051] Furthermore, step 4 includes the following specific steps:

[0052] Construct the dynamic model of the mobile manipulator as follows

[0053]

[0054] In formula (15) and respectively represent the inertia coefficient matrix, centrifugal / Coriolis force coefficient matrix, and gravity torque vector of the mobile manipulator; represents the generalized uncertainty of the dynamics, is the driving torque in the joint space of the mobile manipulator;

[0055] The dynamic feedforward torque vector of the mobile manipulator is equivalently expressed as

[0056]

[0057] The equivalent dynamic model of the mobile robot arm is designed by combining formula (15) and formula (16) as follows:

[0058]

[0059] In formula (17) is a pre-set positive definite diagonal constant matrix, Represents the generalized unknown forces / torques of the mobile manipulator system;

[0060] The design of non-singular terminal sliding mode manifold is as follows:

[0061]

[0062] In formula (18), Λ=diag{Λ1,…,Λ n+3} represents a positive definite diagonal gain matrix, 1<α<2 is a constant coefficient, e=qq d represents the motion error of the mobile robot, where q d is the desired trajectory in its joint space;

[0063] The adaptive super-torsion dynamic sliding mode motion control algorithm of the mobile manipulator based on the above recursive dynamics feedforward is designed as follows:

[0064]

[0065] At the same time, the adaptive law of the gain parameter of the super-torsion dynamic sliding mode reaching law is designed as follows:

[0066]

[0067] In formulas (19) and (20), τ e and τ r represent the supertorsion dynamic sliding mode equivalent control law and the reaching control law respectively, λ1 and λ2 represent the positive definite diagonal gain matrices of the supertorsion dynamic sliding mode reaching control law, sgn(s)=[sign(s1),…,sign(s n+3 )] T ;γ1=diag{γ 1_1 ,…,γ 1_n+3},γ2=diag{γ 2_1 ,…,γ 2_n+3},γ3=diag{γ 3_1 ,…,γ 3_n+3} and μ=diag{μ1,…,μ n+3} are all positive definite diagonal constant coefficient matrices, is a small positive constant, and defines Π3(s)=diag{‖s1‖,…,‖s n+3 ‖}; represents the minimum eigenvalue of the matrix, τ f is the recursive dynamics feedforward torque value of the mobile robot and is calculated by equations (12)-(14).

[0068] Furthermore, the step 5 includes the following specific steps:

[0069] The Lyapunov function is designed as follows:

[0070]

[0071]

[0072] In formulas (21) and (22), in in and Represent the unknown supremum of λ1 and λ2 respectively; define in It is easy to see that the matrix K is positive definite;

[0073] Find the first-order differential of formula (21) with respect to time, and we get

[0074]

[0075] In formula (23) in Represents the largest eigenvalue of the matrix; is the equivalent matrix of K calculated by the simultaneous system dynamic equations (17) and (18). It is easy to know from the adaptive law formula (20) of the reaching law gain parameter that: and are bounded, so Therefore, the motion error e of the mobile robot will converge to the bounded region within a finite time, forming a paradigm framework for robot dynamics feedforward finite time motion control.

[0076] The dynamic sliding mode motion control method of a mobile manipulator based on online identification feedforward of dynamics of the present invention has the following advantages: The online identification algorithm of the dynamics of the mobile manipulator of the present invention can realize the online identification of physically consistent dynamic parameters, effectively overcome the adverse effects of system modeling errors and measurement noises, and improve the modeling accuracy of the dynamic characteristics of the robot; at the same time, an adaptive super-twisting dynamic sliding mode motion control algorithm based on recursive calculation feedforward of dynamics is designed, which not only avoids directly calculating complex dynamic terms such as the inertia matrix and the centrifugal / Coriolis force matrix, but also ensures the continuity of the control law and realizes the high-precision finite-time convergence of the motion error; the finite-time stability of the designed control algorithm eliminates the dependence on prior knowledge such as the system uncertainty and the supremum of external disturbances, which is more conducive to practical engineering applications. The present invention not only proposes a linearized modeling and online identification method that satisfies the physical consistency constraints of robot dynamic parameters, but also forms a paradigm framework for finite-time motion control of robot dynamics feedforward, which can effectively improve the response speed and accuracy of robot motion control. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 is a flowchart of the dynamic sliding mode motion control method of a mobile manipulator based on online identification feedforward of dynamics of the present invention;

[0078] Figure 2 is a schematic diagram of the excitation trajectory of the generalized joint space of the mobile manipulator of the present invention;

[0079] Figure 3 is a schematic diagram of the online identification result of the physically consistent dynamic parameters of the mobile manipulator of the present invention.

[0080] Figure 4 is a schematic diagram of the finite-time convergence effect of the motion control error of the mobile manipulator of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0081] In order to better understand the purpose, structure and function of the present invention, the dynamic sliding mode motion control method of a mobile manipulator based on online identification feedforward of dynamics of the present invention will be further described in detail below with reference to the drawings.

[0082] The present invention first constructs a physically consistent linearized dynamic model of the mobile manipulator and designs an adaptive Kalman filter algorithm to realize the online estimation of dynamic parameters; then designs an adaptive dynamic sliding mode motion control algorithm based on real-time feedforward of recursive calculation of dynamics to realize the high-precision finite-time convergence of the motion error of the mobile manipulator, and finally forms a paradigm framework for finite-time motion control of robot dynamics feedforward.

[0083] As Figure 1 shown, the specific implementation scheme of the present invention includes the following steps:

[0084] Step 1: Design a vehicle-arm coupled linearized dynamic model for the mobile manipulator based on the recursive Newton-Euler algorithm, and transform the physical consistency constraints of the inertia parameters to form a linearized modeling method that satisfies the physical consistency constraints of the robot dynamic parameters, improving the modeling accuracy of the dynamic characteristics;

[0085] Establish the driving force and torque equations required for the movement of the i-th link of the manipulator as follows

[0086]

[0087] In formula (1) i ω i and represent the angular velocity and angular acceleration of the i-th link respectively, represents the linear acceleration of the i-th link; is the regression matrix of the dynamic equation, is the dynamic parameter vector of the i-th link, including the link mass m i , the centroid moment c i = [c ix , c iy , c iz T and the link inertia parameter vector relative to the link coordinate system where the inertia matrix operators and are defined as

[0088]

[0089] Establish the linearized dynamic model of the mobile platform as follows

[0090]

[0091] In formula (3) represents the equivalent driving force and torque of the mobile platform, and represent the translational velocities of the mobile platform along the O w X w Y w Z w axis and the rotational velocity around the O w X w axis, O w Y w axis translation velocities and the rotational velocity around the O w Z w axis in the world coordinate system OXYZ;​ Represents the regression matrix of the mobile platform dynamics equation, Φ p =[m p ,l p T is its inertial parameter vector, including the mass m of the mobile platform p and the moment of inertia l p .

[0092] Based on this, the vehicle-arm coupled linearized dynamics model of the mobile manipulator is constructed as follows

[0093]

[0094] In Equation (4) and q = w x p , w y p , w φ p ,θ1,…,θ n T respectively represent the generalized joint space driving torque and joint position of the mobile manipulator, where θ i ,i = 1,…,n are the joint positions of the manipulator; where the force / moment transfer matrix and respectively represent the pose rotation transformation matrix and the origin translation vector of two adjacent links; s pp =A p , where i x i =[1,0,0] T 、 i y i =[0,1,0] T 、 i z i =[0,0,1] T , the force / moment transfer matrix and represent the regression matrix of the linearized dynamics model of the mobile manipulator, is the unknown dynamics parameter vector of the mobile manipulator. It can be seen that the dynamic coupling influence factor caused by the manipulator movement on the mobile platform is constructed by S pi ,i = 1,…,n, and at the same time, the dynamic coupling influence factor caused by the mobile platform movement on the manipulator is characterized by the velocity transfer of its base coordinate system.

[0095] ​​The dynamic parameters of a mobile manipulator need to satisfy the property constraints of physical consistency, including that the mass of each link is greater than zero, the inertia tensor matrix is symmetric positive definite and satisfies the triangle inequality constraint. Only in this way can the obtained dynamic parameters effectively characterize the dynamic characteristics of the mobile manipulator. Therefore, the parameters to be identified for the i-th link of the manipulator are defined as Ψ i =[ψ im ,ψ ic ,ψ il1 ,ψ il2 ,ψ il3 ,ψ il4 ,ψ il5 ,ψ il 6] T , then its dynamic parameters Φ i satisfying the above physical consistency property constraints are encoded as

[0096]

[0097] In formula (5), ψ ic =[ψ icx ,ψ icy ,ψ icz T represents the mass moment, ε > 0 is a custom small constant, represents a lower triangular matrix. Similarly, the parameters to be identified for the mobile platform are defined as Ψ p =[ψ pm ,ψ pl T , then its dynamic parameters Φ p satisfying the physical consistency constraints are encoded as

[0098]

[0099] Substituting formulas (5) and (6) into formula (4), the physical consistency linearized dynamic model of the mobile robot can be obtained as follows

[0100]

[0101] In formula (7), represents the parameter to be identified, are the reconstructed dynamic parameters of the mobile manipulator satisfying the physical consistency constraints. This method provides a linearized modeling paradigm that satisfies the physical consistency constraints of the robot's dynamic parameters, laying a foundation for the design of subsequent online identification algorithms.

[0102] ​​Step 2: Design an adaptive Kalman filtering algorithm based on the dynamic real-time update of the system modeling error covariance to achieve high-precision online identification of the physical consistency dynamic parameters of the mobile manipulator, effectively overcoming the adverse effects of system modeling errors and measurement noise;

[0103] To solve the physical consistency dynamic parameters of the mobile manipulator online, an adaptive Kalman filtering algorithm based on the dynamic real-time update of the system modeling error covariance is designed as follows

[0104] Step 2.1: Real-time update of dynamic parameters

[0105]

[0106] Step 2.2: Measurement correction of dynamic parameters

[0107]

[0108] In equations (8) and (9) and represent the estimated value of the parameter vector to be identified and the estimated value of its modeling error covariance matrix, Q Φ and R Φ represent the covariance matrices of the system modeling error and the measurement error respectively; is the Kalman filter gain, is the corrected modeling error covariance matrix; is the corrected value of the parameter vector to be identified, so that the physical consistency dynamic parameters of the mobile manipulator can be obtained

[0109] To further overcome the adverse effects of system modeling errors and measurement noise on the identification of dynamic parameters, a dynamic real-time update law for the system modeling error covariance is designed as follows

[0110]

[0111] And the estimated value of the system measurement noise covariance matrix within N sampling periods is designed as follows

[0112]

[0113] Thus, high-precision online identification of the physical consistency dynamic parameters of the mobile manipulator can be achieved.

[0114] Step 3: Based on the online identification results of the physical consistency dynamic parameters, design a recursive dynamic feedforward algorithm for the mobile manipulator, avoiding the direct calculation of complex dynamic terms such as the inertia matrix and the centrifugal / Coriolis force matrix, and effectively improving the calculation efficiency of dynamic real-time feedforward;

[0115] Using the online identification results of the above physical consistency dynamic parameters, the recursive dynamic feedforward algorithm of the mobile manipulator is designed as follows

[0116] Step 3.1: Forward recursively calculate the driving force / moment required for the independent movement of each link

[0117]

[0118] Step 3.2: Backward recursively calculate the driving force / moment required for the coupled movement of each link

[0119]

[0120] Step 3.3: Calculate the dynamic feedforward force / moment value of each joint of the mobile manipulator

[0121]

[0122] In Formulas (12)-(14) and represent the expected link velocities and acceleration values calculated from the expected trajectory of the generalized joint space of the mobile manipulator Thus, the dynamic feedforward torque vector of the mobile manipulator can be obtained as This recursive dynamic feedforward algorithm avoids the direct calculation of complex dynamic terms such as the inertia matrix and the centrifugal / Coriolis force matrix, effectively improving the calculation efficiency of real-time dynamic feedforward

[0123] Step 4: Design an adaptive dynamic sliding mode motion control algorithm for the mobile manipulator based on recursive dynamic feedforward, and an adaptive algorithm for the gain parameters of the super-twisting dynamic sliding mode reaching law, to achieve high-precision finite-time convergence of the motion error of the mobile manipulator and ensure the continuity of the control law to protect the actuator and for practical engineering applications

[0124] Construct the dynamic model of the mobile manipulator as follows

[0125]

[0126] In Formula (15) and represent the inertia coefficient matrix, the centrifugal / Coriolis force coefficient matrix, and the gravity torque vector of the mobile manipulator respectively represents the generalized uncertainty of the dynamics is the driving torque in the joint space of the mobile manipulator

[0127] The dynamic feedforward torque vector of the mobile manipulator can be equivalently expressed as

[0128]

[0129] In order to avoid direct calculation of complex dynamic terms such as the inertia matrix of the mobile robot in the control algorithm, the equivalent dynamic model of the mobile robot is designed by combining formula (15) and formula (16) as follows:

[0130]

[0131] In formula (17) is a pre-set positive definite diagonal constant matrix, Represents the generalized unknown forces / torques of the mobile robotic arm system.

[0132] In order to achieve finite time convergence of the motion error, a non-singular terminal sliding mode manifold is designed as follows

[0133]

[0134] In formula (18), Λ=diag{Λ1,…,Λ n+3} represents a positive definite diagonal gain matrix, 1<α<2 is a constant coefficient, e=qq d represents the motion error of the mobile robot, where q d is the desired trajectory in its joint space.

[0135] Based on this, an adaptive super-torsion dynamic sliding mode motion control algorithm for a mobile manipulator based on the above recursive dynamics feedforward is designed as follows

[0136]

[0137] At the same time, the adaptive law of the gain parameter of the super-torsion dynamic sliding mode reaching law is designed as follows

[0138]

[0139] In formulas (19) and (20), τ e and τ r represent the supertorsion dynamic sliding mode equivalent control law and the reaching control law respectively, λ1 and λ2 represent the positive definite diagonal gain matrices of the supertorsion dynamic sliding mode reaching control law, sgn(s)=[sign(s1),…,sign(s n+3 )] T ;γ1=diag{γ 1_1 ,…,γ 1_n+3},γ2=diag{γ 2_1 ,…,γ 2_n+3},γ3=diag{γ 3_1 ,…,γ 3_n+3} and μ=diag{μ1,…,μ n+3} are all positive definite diagonal constant coefficient matrices, is a relatively small positive constant, and meanwhile define Π3(s) = diag{‖s1‖, …, ‖s n+3 ‖}; represents the minimum eigenvalue of the matrix, and τ f is the recursive dynamic feed - forward torque value of the mobile manipulator and is calculated by formulas (12) - (14). This algorithm can achieve high - precision finite - time convergence of the motion error of the mobile manipulator and ensure the continuity of the control law to protect the actuators and for practical engineering applications.

[0140] Step 5: Adopt the Lyapunov design method to achieve the finite - time stability of the entire closed - loop system for the dynamic online feed - forward - adaptive dynamic sliding - mode control, eliminate the dependence on the prior knowledge such as the system uncertainty and the supremum of external disturbances, and form a paradigm framework for the finite - time motion control of the robot dynamics feed - forward.

[0141] To prove the finite - time stability of the entire closed - loop system for the above - mentioned dynamic online feed - forward - adaptive dynamic sliding - mode control, design the Lyapunov function as follows

[0142]

[0143] In formulas (21) and (22) where where and represent the unknown suprema of λ1 and λ2 respectively; meanwhile define where It is easy to know that the matrix K is positive definite.

[0144] Take the first - order derivative of formula (21) with respect to time, and we can get

[0145]

[0146] In formula (23) where represents the maximum eigenvalue of the matrix; is the equivalent matrix of K calculated by combining the system dynamic equations formulas (17) and (18). From the adaptive law formula (20) of the reaching law gain parameter, it is easy to know that and are both bounded, so and is bounded.

[0147] Therefore, the motion error of the mobile manipulator will converge to a bounded region within a finite time. Meanwhile, the finite-time stability of the designed control system does not depend on prior knowledge such as the supremum of system uncertainties and external disturbances, and finally a paradigm framework for finite-time motion control of robot dynamics with feedforward is formed.

[0148] Embodiment

[0149] The flow of the dynamic sliding mode motion control method for a mobile manipulator based on online identification feedforward of dynamics in the present invention is as Figure 1 shown. The specific object to be implemented is a mobile manipulator composed of an omnidirectional mobile platform and a six-degree-of-freedom manipulator, and its kinematic parameters are d b = 0.140m, d1 = 0.140m, a2 = 0.375m, a3 = 0.345m, d4 = 0.122m, d5 = 0.122m, d6 = 0.083m, and its physically consistent dynamic parameters will be directly obtained by the designed online identification algorithm.

[0150] During the implementation of the present invention, in order to meet the excitation conditions of the dynamic characteristics of the mobile manipulator, a periodic excitation trajectory in the joint space is designed based on the fifth-order Fourier series as follows

[0151]

[0152] where q di0 represents the initial position of the i-th joint, ω f = 0.1π represents the fundamental frequency of the excitation trajectory, a ik and b ik , k = 1,…,5 represent the constant coefficients of the excitation trajectory; thus, the excitation trajectory in the joint space of the mobile manipulator is obtained as Figure 2 shown.

[0153] The dynamic sliding mode motion control law for a mobile manipulator based on online identification feedforward of dynamics in the present invention is shown in Formulas (8)-(14) and Formulas (18)-(20), where the relevant parameter values of the control algorithm are: γ3 = diag{[0.1,0.1,5,3,5,3,1,0.5,0.2] T}, Λ = diag{[1,1,1,1,1,1,1,1,1] T}, μ = 10 -4 ×diag{[1,1,1,1,1,1,1,1,1] T}, σ = 10 -3 ×diag{[1,1,1,1,1,1,1,1,1] T}, γ1 = diag{[1.5, 1.5, 1.5, 1.2, 1.2, 1.2, 1, 1, 1] T}, γ2 = diag{[0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.02, 0.02, 0.02] T}, ε = 0.001, α = 1.8, δ(0) = diag{[1.2, 1.2, 1.2, 1.5, 1.5, 1.5, 1, 1, 1] T} R Φ = diag{[10, 10, 100, 100, 100, 100, 100, 100, 100] T} Thus, the implementation effects of the online identification feedforward and motion control of the mobile manipulator dynamics can be obtained, Figure 3 indicating that the physical consistency dynamic parameters of the mobile manipulator can converge from zero initial values to equilibrium values, proving the effectiveness of the designed dynamic online identification algorithm; at the same time Figure 4 indicating that the actual trajectory in the generalized joint space of the mobile manipulator can quickly converge to the desired trajectory within a finite time, effectively improving the response speed and accuracy of the operating motion of the mobile manipulator.

[0154] It can be understood that the present invention is described by means of some embodiments. Those skilled in the art know that, without departing from the spirit and scope of the present invention, various changes or equivalent replacements can be made to these features and embodiments. Additionally, under the teaching of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application belong to the scope protected by the present invention.

Claims

1. A dynamic sliding mode motion control method for a mobile manipulator based on dynamics online identification feedforward, characterized in that: The following steps are involved: Step 1: Design a vehicle-arm coupled linearized dynamic model of the mobile robot based on the recursive Newton-Euler algorithm, and transform the physical consistency constraints of the inertia parameters to form a linearized modeling method that satisfies the physical consistency constraints of the robot dynamic parameters; Step 2: Design an adaptive Kalman filter algorithm based on the dynamic real-time update of the system modeling error covariance to achieve high-precision online identification of the physical consistency dynamic parameters of the mobile robot arm; Step 3: Based on the online identification results of the physically consistent dynamic parameters, a recursive dynamic feedforward algorithm for the mobile robot is designed; Step 4: Design an adaptive dynamic sliding mode motion control algorithm for the mobile manipulator based on recursive dynamics feedforward and an adaptive algorithm for the gain parameters of the hypertorsion dynamic sliding mode reaching law to achieve high-precision finite-time convergence of the motion error of the mobile manipulator; Step 5: Use the Lyapunov design method to achieve the finite-time stability of the entire closed-loop system of dynamic online feedforward-adaptive dynamic sliding mode control, forming a paradigm framework for robot dynamic feedforward finite-time motion control.

2. The dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward according to claim 1 is characterized in that: The step 1 comprises the following specific steps: Based on the recursive Newton-Euler algorithm, the driving force required for the motion of the i-th link of the robot arm is established and torque The equation is as follows In formula (1) i ω i and represent the angular velocity and angular acceleration of the i-th connecting rod, respectively. represents the linear acceleration of the i-th link; is the regression matrix of the dynamic equation, is the dynamic parameter vector of the i-th connecting rod, including the connecting rod mass m i , connecting rod centroid moment c i =[c ix ,c iy ,c iz ] T and the connecting rod inertia parameter vector relative to the connecting rod coordinate system The inertia matrix Operator and is defined as: The linearized dynamic model of the mobile platform is established as follows: In formula (3) represents the equivalent driving force and torque of the mobile platform, and Represents the mobile platform in the world coordinate system O w X w Y w Z w The lower edge O w X w Axis, O w Y w Axis translation speed and rotation speed w Z w Shaft rotation speed; represents the regression matrix of the mobile platform dynamics equation, Φ p =[m p ,l p ] T is its inertial parameter vector, including the mass m of the mobile platform p and moment of inertia l p ; Based on this, the vehicle-arm coupling linearized dynamic model of the mobile robotic arm is constructed as follows: In formula (4) and q=[ w x p , w y p , w φ p ,θ1,…,θ n ] T Represent the generalized joint space driving torque and joint position of the mobile manipulator, respectively, where θ i ,i=1,…,n is the joint position of the robot arm; The force / torque transfer matrix and Represent the position rotation transformation matrix and origin translation vector of two adjacent links respectively; in i x i =[1,0,0] T , i y i =[0,1,0] T , i z i =[0,0,1] T , force / torque transfer matrix and represents the linearized dynamic model regression matrix of the mobile manipulator, is the unknown dynamic parameter vector of the mobile manipulator; The dynamic parameters of the mobile robot arm need to satisfy the physical consistency constraints, that is, the mass of each link is greater than zero, the inertia tensor matrix is ​​symmetric and positive and satisfies the triangular inequality constraints; the parameter to be identified of the i-th link of the robot arm is defined as Ψ i =[ψ im ,ψ ic ,ψ il1 ,ψ il2 ,ψ il3 ,ψ il4 ,ψ il5 ,ψ il6 ] T , then its dynamic parameter Φ satisfies the above physical consistency constraint i Encoded as In formula (5), ψ ic =[ψ icx ,ψ icy ,ψ icz ] T represents the mass moment, ε>0 is a custom small constant, Represents a lower triangular matrix; similarly, define the parameters to be identified of the mobile platform Ψ p =[ψ pm ,ψ pl ] T , then its dynamic parameter Φ satisfies the physical consistency constraint p is encoded as: Substituting formulas (5) and (6) into formula (4), the physical consistency linearized dynamic model of the mobile machinery can be obtained as follows: In formula (7) represents the parameter to be identified, are the reconstructed dynamic parameters of the mobile manipulator that satisfy the physical consistency constraints.

3. The dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward according to claim 2 is characterized in that: The step 2 comprises the following specific steps: Step 2.1: Real-time update of kinetic parameters; Step 2.2: Kinetic parameter measurement correction.

4. The dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward according to claim 3 is characterized in that: The kinetic parameters of step 2.1 are updated in real time as follows:

5. The dynamic sliding mode motion control method for a mobile manipulator based on dynamics online identification feedforward according to claim 3 is characterized in that: The modification of the kinetic parameter measurement in step 2.2 is as follows: In formula (8) and formula (9), and Represents the estimated value of the parameter vector to be identified and the estimated value of the modeling error covariance matrix, Q Φ and R Φ Represent the covariance matrices of system modeling error and measurement error respectively; is the Kalman filter gain, is the corrected modeling error covariance matrix; is the correction value of the parameter vector to be identified, and the physical consistency dynamic parameters of the mobile manipulator are obtained Design system modeling error covariance dynamic real-time update law as follows: And design the estimated value of the system measurement noise covariance matrix within N sampling periods as follows: This enables high-precision online identification of the physical consistency dynamic parameters of the mobile robot.

6. The dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward according to claim 2 is characterized in that: The step 3 comprises the following specific steps: Step 3.1: Forward recursion calculates the driving force / torque required for each link to move independently; Step 3.2: Reverse recursion to calculate the driving force / torque required for the motion of each link: Step 3.3: Calculate the dynamic feedforward force / torque value of each joint of the mobile robot: In formula (12)-(14) and Represents the expected trajectory of the generalized joint space of the mobile robot The expected velocity and acceleration of the connecting rod are calculated, and the dynamic feedforward torque vector of the mobile manipulator is obtained as follows:

7. The dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward according to claim 1 is characterized in that: The step 4 comprises the following specific steps: Construct the dynamic model of the mobile robot as follows In formula (15) and They represent the inertia coefficient matrix, centrifugal force / Coriolis force coefficient matrix and gravity moment vector of the mobile manipulator respectively; represents the generalized uncertainty of the dynamics, is the driving torque that moves the manipulator’s joint space; Dynamic feed-forward torque vector of mobile manipulator The equivalent expression is The equivalent dynamic model of the mobile robot arm is designed by combining formula (15) and formula (16) as follows: In formula (17) is a pre-set positive definite diagonal constant matrix, Represents the generalized unknown forces / torques of the mobile manipulator system; The design of non-singular terminal sliding mode manifold is as follows: In formula (18), Λ=diag{Λ1,…,Λ n+3 } represents a positive definite diagonal gain matrix, 1<α<2 is a constant coefficient, e=qq d represents the motion error of the mobile robot, where q d is the desired trajectory in its joint space; The adaptive super-torsion dynamic sliding mode motion control algorithm of the mobile manipulator based on the above recursive dynamics feedforward is designed as follows: At the same time, the adaptive law of the gain parameter of the super-torsion dynamic sliding mode reaching law is designed as follows: In formulas (19) and (20), τ e and τ r represent the supertorsion dynamic sliding mode equivalent control law and the reaching control law respectively, λ1 and λ2 represent the positive definite diagonal gain matrices of the supertorsion dynamic sliding mode reaching control law, sgn(s)=[sign(s1),…,sign(s n+3 )] T ;γ1=diag{γ 1_1 ,…,γ 1_n+3 },γ2=diag{γ 2_1 ,…,γ 2_n+3 },γ3=diag{γ 3_1 ,…,γ 3_n+3 } and μ=diag{μ1,…,μ n+3 } are all positive definite diagonal constant coefficient matrices, is a small positive constant, and defines ∏3(s)=diag{‖s1‖,…,‖s n+3 ‖}; represents the minimum eigenvalue of the matrix, τ f is the recursive dynamics feedforward torque value of the mobile robot and is calculated by equations (12)-(14).

8. The dynamic sliding mode motion control method for a mobile manipulator based on dynamics online identification feedforward according to claim 7 is characterized in that: The step 5 comprises the following specific steps: The Lyapunov function is designed as follows: In formulas (21) and (22), in in and Represent the unknown supremum of λ1 and λ2 respectively; define in It is easy to see that the matrix K is positive definite; Find the first-order differential of formula (21) with respect to time, and we get In formula (23) in Represents the largest eigenvalue of the matrix; is the equivalent matrix of K calculated by the simultaneous system dynamic equations (17) and (18). It is easy to know from the adaptive law formula (20) of the reaching law gain parameter that: and are bounded, so and bounded; Therefore, the motion error e of the mobile robot will converge to the bounded region within a finite time, forming a paradigm framework for robot dynamics feedforward finite-time motion control.

Images