Dynamic mechanical arm dynamic sliding mode motion control method based on dynamic online identification feedforward

Abstract

The invention belongs to the field of robot motion control, and discloses a dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward, which comprises the following steps of: 1, designing a physical consistency linearization coupling dynamic model of the mobile manipulator; 2, designing a self-adaptive Kalman filtering algorithm based on modeling error covariance dynamic updating, and realizing high-precision online identification of the physical consistency kinetic parameters of the mobile mechanical arm; 3, designing a recursive dynamics feedforward algorithm of the mobile mechanical arm; 4, a self-adaptive over-torque dynamic sliding mode control algorithm based on recursive dynamics feedforward is designed, the continuity of the control law is guaranteed, and high-precision finite time convergence of the motion error of the mobile mechanical arm is achieved; and 5, the stability of the controller is achieved through a Lyapunov design method, a paradigm framework of robot dynamics feedforward finite time motion control is formed, and the response speed and precision of robot motion control can be effectively improved.

Description

Technical Field

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

[0002] The mobile robot composed of a mobile platform and an operating arm has a wide range of motion and flexible operation forms. It is gradually being used in industrial scenarios such as spraying, inspection, loading and unloading, and service areas such as picking, opening doors, and collaborative handling. However, the motion control performance of the mobile robot is still limited by the complex system dynamics coupling and its uncertainty. For example, the involved speed of the mobile platform movement causes centrifugal force / Coriolis force on the robot arm, and the movement of the robot arm will also cause the inertial force and centrifugal force / Coriolis force of the mobile platform to change. In addition, when the mobile robot handles unknown loads in a complex environment or faces uneven roads, its dynamic properties such as mass, center of gravity and inertia tensor will also change. Therefore, constructing a vehicle-arm coupling dynamics model of the mobile robot and developing advanced motion control algorithms for the above problems are the keys to its efficient and high-quality operation.

[0003] At present, most robot motion controllers require dynamic calculation models to compensate for the influence of time-varying nonlinear strongly coupled dynamic characteristics on motion control performance. Due to manufacturing errors and CAD modeling errors, the robot dynamic parameters derived by CAD / CAM and other methods are inaccurate and rarely used directly for controller design. Based on the input and output of the robot system, there are currently a variety of numerical calculation methods such as least squares method and particle swarm algorithm used to identify the robot's dynamic model, but most of these methods can only realize offline identification of dynamic parameters, and it is difficult to directly use them for mobile robot systems whose dynamic characteristics change in real time due to complex tasks and unstructured environments. In addition, although recursive least squares method, adaptive neural network, adaptive observer, etc. can realize 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 dynamic parameters do not have physical consistency, so it is still difficult to achieve high-precision dynamic feedforward control of mobile robot arms. In response to 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, but rarely optimize the design convergence speed. In addition, the controller stability design relies on prior knowledge such as the supremum of system uncertainty that is difficult to obtain directly, as well as the complex calculation of the dynamics of redundant robot systems such as mobile robotic arms, which limit the practical application of the above control methods. Summary of the invention

[0004] The present invention aims to provide a dynamic sliding mode motion control method for a mobile robot arm based on dynamic online identification feedforward to solve the above-mentioned technical problems.

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

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

[0007] 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;

[0008] 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;

[0009] 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;

[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 hypertorsion 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 dynamic online feedforward-adaptive dynamic sliding mode control, forming a paradigm framework for robot dynamic feedforward finite-time motion control.

[0012] Furthermore, the step 1 includes the following specific steps:

[0013] 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

[0014]

[0015] In formula (1) i 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 dynamics 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:

[0016]

[0017] The linearized dynamic model of the mobile platform is established as follows:

[0018]

[0019] 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 ;

[0020] Based on this, the vehicle-arm coupling linearized dynamic model of the mobile robotic arm is constructed as follows:

[0021]

[0022] 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; S pp =a p , in i ix 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;

[0023] 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 physical consistency constraint i 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 Ψ p =[ψpm ,ψ pl ] T , then its dynamic parameter Φ 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 machinery can be obtained as follows:

[0028]

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

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

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

[0032] Step 2.2: Kinetic parameter measurement correction.

[0033] Furthermore, the kinetic parameters in step 2.1 are updated in real time as follows:

[0034]

[0035] Furthermore, the kinetic parameter measurement in step 2.2 is modified as follows:

[0036]

[0037] 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

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

[0039]

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

[0041]

[0042] This enables high-precision online identification of the physical consistency dynamic parameters of the mobile robot.

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

[0044] Step 3.1: Forward recursion calculates the driving force / torque required for each link to move independently;

[0045]

[0046] Step 3.2: Reverse recursion to calculate the driving force / torque required for the motion of each link:

[0047]

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

[0049]

[0050] In formula (12)-(14) i ω id , 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:

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

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

[0053]

[0054] 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;

[0055] Dynamic feed-forward torque vector of mobile manipulator The equivalent expression is

[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 Represents the positive definite diagonal gain matrix of the supertorsion dynamic sliding mode approaching control law, sgn(s)=[sign(s 1 ),…,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{‖s 1 ‖,…,‖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 Represents λ 1 and λ 2 The unknown supremum of in It is easy to see that the matrix K is positive definite;

[0073] Taking the first-order differential of formula (21) with respect to time, 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 dynamic online identification feedforward of the present invention has the following advantages: the dynamic online identification algorithm of the mobile manipulator of the present invention can realize the online identification of physical consistency dynamic parameters, effectively overcome the adverse effects of system modeling errors and measurement noise, and improve the modeling accuracy of the robot's dynamic characteristics; at the same time, an adaptive super-torsion dynamic sliding mode motion control algorithm based on dynamic recursive calculation feedforward is designed, which not only avoids directly calculating complex dynamic terms such as inertia matrix and centrifugal force / Coriolis force matrix, but also ensures the continuity of the control law and realizes high-precision finite time convergence of motion error; the finite time stability of the designed control algorithm eliminates the dependence on prior knowledge such as system uncertainty and external disturbance supremum, which is more conducive to practical engineering applications. The present invention not only proposes a linearized modeling and online identification method that meets the physical consistency constraints of robot dynamic parameters, but also forms a paradigm framework of robot dynamic feedforward finite time motion control, which can effectively improve the response speed and accuracy of robot motion control. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0079] Figure 3 It is a schematic diagram of the online identification results of the physical consistency dynamic parameters of the mobile robot arm of the present invention.

[0080] Figure 4 It 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

[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 dynamic online identification feedforward of the present invention is further described in detail below in conjunction with the accompanying drawings.

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

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

[0084] Step 1: Design the vehicle-arm coupling 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's dynamic parameters, thereby improving the accuracy of dynamic characteristic modeling;

[0085] 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

[0086]

[0087] 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

[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 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 .

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

[0093]

[0094] 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; s pp =A p , 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 robot. It can be seen that the dynamic coupling factor caused by the robot movement on the mobile platform is composed of S pi , i=1,…,n is constructed, and the dynamic coupling influence factor caused by the movement of the mobile platform on the robotic arm is characterized by the velocity transfer of its base coordinate system.

[0095] The dynamic parameters of the mobile manipulator need to satisfy the physical consistency constraints, including that the mass of each link is greater than zero, the inertia tensor matrix is ​​symmetric and positive, and satisfies the triangular inequality constraints. The dynamic parameters obtained in this way can effectively characterize the dynamic characteristics of the mobile manipulator. Therefore, the parameter to be identified of the i-th link of the manipulator is defined as Ψ i =[ψ im ,ψ ic ,ψ il1 ,ψ il2 ,ψ il3 ,ψ il4 ,ψ il5 ,ψ il 6] T , then its dynamic parameter Φ satisfies the above physical consistency property constraints i 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, define the parameters to be identified of the mobile platform Ψ p =[ψ pm ,ψ pl ] T , then its dynamic parameter Φ satisfies the physical consistency constraint p Encoded as

[0098]

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

[0100]

[0101] In formula (7) represents the parameter to be identified, The reconstructed dynamic parameters of the mobile robot satisfy the physical consistency constraint. This method provides a linear modeling paradigm that satisfies the physical consistency constraint of the robot dynamic parameters, laying the foundation for the subsequent online identification algorithm design.

[0102] 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, effectively overcoming the adverse effects of system modeling errors and measurement noise;

[0103] In order to solve the physical consistency dynamic parameters of the mobile robot online, an adaptive Kalman filter 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 kinetic parameters

[0105]

[0106] Step 2.2: Kinetic parameter measurement correction

[0107]

[0108] 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, so that the physical consistency dynamic parameters of the mobile robot can be obtained

[0109] In order to further overcome the adverse effects of system modeling errors and measurement noise on dynamic parameter identification, a dynamic real-time update law of system modeling error covariance is designed as follows:

[0110]

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

[0112]

[0113] This enables high-precision online identification of the physical consistency dynamic parameters of a mobile robot.

[0114] 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 to avoid the direct calculation of complex dynamic terms such as the inertia matrix and the centrifugal force / Coriolis force matrix, effectively improving the calculation efficiency of the real-time dynamic feedforward.

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

[0116] Step 3.1: Forward recursion to calculate the driving force / torque required for each link to move independently

[0117]

[0118] Step 3.2: Reverse recursion to calculate the driving force / torque required for the motion of each link

[0119]

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

[0121]

[0122] In formula (12)-(14) and Represents the expected trajectory of the generalized joint space of the mobile robot The expected speed and acceleration of the connecting rod are calculated. Therefore, the dynamic feedforward torque vector of the mobile robot is: This recursive dynamics feedforward algorithm avoids the direct calculation of complex dynamic terms such as the inertia matrix and the centrifugal force / Coriolis force matrix, effectively improving the computational efficiency of dynamics real-time feedforward.

[0123] 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 and ensure the continuity of the control law to protect the drive and practical engineering applications;

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

[0125]

[0126] 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 robot's joint space.

[0127] Dynamic feed-forward torque vector of mobile manipulator It 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 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 Represents the positive definite diagonal gain matrix of the supertorsion dynamic sliding mode approaching control law, sgn(s)=[sign(s 1 ),…,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{‖s 1 ‖,…,‖s n+3 ‖}; represents the minimum eigenvalue of the matrix, τ f is the recursive dynamics feedforward torque value of the mobile manipulator and is calculated by equations (12)-(14). The 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 drive and practical engineering applications.

[0140] Step 5: The Lyapunov design method is used to achieve the finite-time stability of the entire closed-loop system of dynamic online feedforward-adaptive dynamic sliding mode control, eliminating the dependence on prior knowledge such as system uncertainty and external disturbance supremum, and forming a paradigm framework for robot dynamic feedforward finite-time motion control.

[0141] In order to prove the finite-time stability of the above dynamic online feedforward-adaptive dynamic sliding mode control of the entire closed-loop system, the Lyapunov function is designed as follows

[0142]

[0143] In formulas (21) and (22), in in and Represents λ 1 and λ 2 The unknown supremum of in It is easy to see that the matrix K is positive definite.

[0144] Taking the first-order differential of formula (21) with respect to time, we can obtain

[0145]

[0146] 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.

[0147] Therefore, the motion error of the mobile robot will converge to a bounded region within a finite time. At the same time, the finite-time stability of the designed control system does not depend on prior knowledge such as system uncertainty and the supremum of external disturbances, ultimately forming a paradigm framework for robot dynamics feedforward finite-time motion control.

[0148] Example

[0149] The process of the dynamic sliding mode motion control method of the mobile manipulator based on dynamic online identification feedforward of the present invention is as follows: Figure 1 As shown, the specific implementation object is a mobile robot composed of an omnidirectional mobile platform and a six-degree-of-freedom robot arm, and its kinematic parameters are d b =0.140m,d 1 =0.140m, a 2 =0.375m, a 3 =0.345m, d 4 =0.122m,d 5 =0.122m,d 6 =0.083m, and its physically consistent dynamic parameters will be directly obtained by the designed online identification algorithm.

[0150] In the implementation process of the present invention, in order to meet the excitation conditions of the dynamic characteristics of the mobile robot, the periodic excitation trajectory of its 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 represents the constant coefficient of the excitation trajectory; thus, the excitation trajectory of the mobile manipulator joint space is obtained as follows Figure 2 shown.

[0153] The dynamic sliding mode motion control law of the mobile manipulator based on dynamic online identification feedforward of the present invention is shown in formulas (8)-(14) and (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}, The implementation effect of online identification feedforward and motion control of mobile robot dynamics can be obtained. Figure 3 It shows that the physical consistency dynamic parameters of the mobile robot can converge from zero initial values ​​to equilibrium values, proving the effectiveness of the designed dynamic online identification algorithm; Figure 4 This shows that the actual trajectory of the generalized joint space of the mobile robot arm can quickly converge to the desired trajectory within a limited time, effectively improving the response speed and accuracy of the mobile robot arm's operating motion.

[0154] It is to be understood that the present invention is described by some embodiments, and it is known to those skilled in the art that various changes or equivalent substitutions may be made to these features and embodiments without departing from the spirit and scope of the present invention. In addition, under the teachings of the present invention, these features and embodiments may be modified to adapt to specific circumstances 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 are within the scope of protection of the present invention.

Claims

1. A dynamic sliding mode motion control method for a mobile manipulator based on dynamic online identification feedforward, characterized in that: The following steps are involved: Step 1: Design the vehicle-arm coupling 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 dynamics 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 1 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 1 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) i ω id , and Represents the expected trajectory q in the generalized joint space of the mobile robot d , 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 dynamics 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 dynamic online identification feedforward according to claim 1 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; Taking the first-order differential of formula (21) with respect to time, 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 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