Mechanical arm trajectory tracking method and equipment based on geometric model predictive control

Abstract

The invention provides a mechanical arm trajectory tracking method and device based on geometric model predictive control. The mechanical arm trajectory tracking method comprises the steps that a mechanical arm dynamic model is established and linearized in a task space; defining a left invariant tracking error based on the Lie group theory, and linearizing the left invariant tracking error in the tangent space of the Lie group to obtain an error kinetic equation; constructing a nominal geometric model predictive controller, establishing an augmented state space and constructing a cost function; constructing a tubular geometric model prediction controller, and solving a finite time domain discrete optimization problem by adopting a minimum robust positive invariant set theory; a weighted whole-body controller is integrated, and predetermined frequency control is carried out on trajectory tracking of the mechanical arm by solving a convex quadratic programming problem.

Description

Technical Field

[0001] The embodiments of the present invention relate to the technical field of robotic arm control, and in particular to a robotic arm trajectory tracking method and device based on geometric model predictive control. Background Art

[0002] As an important automated equipment in industrial manufacturing, medical surgery, and aerospace missions, the trajectory tracking control performance of the manipulator directly determines the success or failure of the mission. However, the high nonlinearity of the dynamic system and external environmental factors (such as load changes and friction) pose significant challenges to control stability and robustness. At the same time, strict mechanical limitations and safety constraints must be met. In existing technologies, joint space control requires mapping the task trajectory through inverse kinematics calculations, which is computationally burdensome and inefficient in continuous high-precision tracking scenarios. Although task space control avoids inverse kinematics calculations, it is affected by internal parameter uncertainty and external disturbances, resulting in insufficient tracking performance. Traditional optimization-based model predictive control (MPC) is mostly modeled in Euclidean space, which is difficult to handle non-Euclidean SO(3) and SE(3) manifolds where the manipulator's rigid body posture is located (Euler angles have singularities and quaternions have redundancy). In addition, joint space modeling still requires inverse kinematics calculations, which is computationally inefficient and has room for improvement in robustness when processing input and state constraints and external disturbances. Although geometric model predictive control has been applied to fields such as unmanned ships, it has not been widely used in the robust control of robotic arm task space, and there is room for improvement in constraint and disturbance processing. A new method is urgently needed to achieve high-precision and robust trajectory tracking control. Summary of the Invention

[0003] In response to the above-mentioned problems existing in the prior art, an embodiment of the present invention provides a robot arm trajectory tracking method and device based on geometric model predictive control.

[0004] In the first aspect, an embodiment of the present invention provides a robot arm trajectory tracking method based on geometric model predictive control, including: establishing and linearizing a robot arm dynamics model in the task space; defining a left-invariant tracking error based on Lie group theory, and linearizing the left-invariant tracking error in the tangent space of the Lie group to obtain an error dynamics equation; constructing a nominal geometric model predictive controller, establishing an augmented state space and constructing a cost function; constructing a tubular geometric model predictive controller, and using the minimum robust positive invariant set theory to solve a finite time domain discrete optimization problem; integrating a weighted whole-body controller to perform predetermined frequency control on the robot arm trajectory tracking by solving a convex quadratic programming problem.

[0005] Based on the content of the above method embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, wherein the robot arm dynamics model is established and linearized in the task space, including: using the special Euclidean group SE (3) manifold to represent the end effector posture, establishing the mapping relationship between the joint space and the task space spinor through the Jacobian matrix, defining the rate of change of the joint angle within the sampling interval, and linearizing the robot arm dynamics model in the task space into a linear non-homogeneous equation:

[0006]

[0007] u=τ

[0008] F t =J(q)M(q) -1

[0009] b t =J(q)M(q) -1 (wG(q))

[0010] Among them, H t is the task space inertia matrix; F t is the force transmission matrix; b t is the bias term; V t is the actual motion spinor; u is the control input; J(q) is the Jacobian matrix; q is the joint angle; is the Coriolis force and centrifugal force; M(q) is the inertia matrix; M(q) -1 is the inverse of the inertia matrix; J(q) -1 is the inverse of the Jacobian matrix; τ is the joint torque; w is the external disturbance; G(q) is the gravity; · is the sign of the first-order derivative with respect to time.

[0011] Based on the content of the above method embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention defines the left invariant tracking error based on Lie group theory, and linearizes the left invariant tracking error in the tangent space of the Lie group to obtain the error dynamics equation, including:

[0012]

[0013] Among them, t is the left invariant tracking error; g d,t is the expected trajectory; -1 is the inverse sign; g t is the actual trajectory; is the linearized spinor error expressed in Lie algebra; is the adjoint operator; ∧ is the symbol of the antisymmetric matrix; ω is the angular velocity of the end effector; v is the linear velocity of the end effector; V d,t is the desired motion spinor.

[0014] Based on the content of the above method embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, the construction of the nominal geometric model predictive controller includes:

[0015]

[0016] in, is the nominal geometric model predictive controller; t is the time; ΔT is the time difference; is the stage cost function; is the terminal cost function; It is the nominal state; is the nominal input; Q is the symbol of Q norm; R is the symbol of R norm; P is the symbol of P norm; To define symbols.

[0017] Based on the content of the above method embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, the construction of the tubular geometric model predictive controller includes:

[0018]

[0019] Among them, min is the minimum value symbol; u k is the actual input; is the cost function; N is the prediction time domain; k is the time step; is the nominal state; y k is the actual state; T is the matrix transpose symbol; Q is the state weight; is the nominal input; R is the input weight; is the terminal state; P is the terminal weight; is the state transfer matrix; is the control input matrix; C k is the output matrix; is the interference input matrix; A k is the state transfer matrix before coordinate transformation; B k h is the control input matrix before coordinate transformation; k Output matrix before coordinate transformation; d k is the feedforward compensation matrix; is the state constraint set; Subtraction for the Minkowski set; is a robust invariant set; is the addition of the Minkowski set; Ω tube is the terminal set; U tubeTo tighten the input constraint set; K k is the feedback coefficient; j is the power; s is the number of iteration steps; W k is the disturbance invariant set; K is the state feedback control gain matrix.

[0020] Based on the content of the above method embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, the integrated weighted whole body controller includes:

[0021]

[0022] Ax=b,low≤Ax≤up

[0023]

[0024] Among them, w i is the i-th weight coefficient; |||| is the norm symbol; A i is the coefficient matrix of the i-th task; b i is the constant of the i-th task; A is the coefficient matrix; x is the decision variable; b is a constant; low is the lower bound of the constraint; pos is the end effector position; ori is the end effector posture; τ is the input torque; up is the upper bound of the constraint; ·· is the sign of the second-order derivative of the joint angle with respect to time.

[0025] In the second aspect, an embodiment of the present invention provides a robotic arm trajectory tracking system based on geometric model predictive control, including: a disturbance observer for obtaining observation values ​​output by the robotic arm and the whole-body controller; a robotic arm for executing control results; a geometric model predictive controller for enhancing system robustness; and a whole-body controller for loading corresponding programs to implement the robotic arm trajectory tracking method based on geometric model predictive control as described in any of the aforementioned method embodiments.

[0026] In the third aspect, an embodiment of the present invention provides a robot arm trajectory tracking device based on geometric model predictive control, including: a first main module, used to establish and linearize the robot arm dynamics model in the task space; a second main module, used to define the left invariant tracking error based on Lie group theory, and linearize the left invariant tracking error in the tangent space of the Lie group to obtain the error dynamics equation; a third main module, used to build a nominal geometric model predictive controller, establish an augmented state space and construct a cost function; a fourth main module, used to build a tubular geometric model predictive controller, and use the minimum robust positive invariant set theory to solve the finite time domain discrete optimization problem; a fifth main module, used to implement an integrated weighted whole-body controller, and perform predetermined frequency control on the robot arm trajectory tracking by solving a convex quadratic programming problem.

[0027] In a fourth aspect, an embodiment of the present invention provides an electronic device, including:

[0028] At least one processor, at least one memory and a communication interface; wherein,

[0029] The processor, memory and communication interface communicate with each other;

[0030] The memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the robot arm trajectory tracking method based on geometric model predictive control provided by any one of the various implementation methods of the first aspect.

[0031] In the fifth aspect, an embodiment of the present invention provides a non-transitory computer-readable storage medium, which stores computer instructions, and the computer instructions enable a computer to execute the robot arm trajectory tracking method based on geometric model predictive control provided by any one of the various implementation methods of the first aspect.

[0032] The robot arm trajectory tracking method and device based on geometric model predictive control provided by the embodiments of the present invention avoid inverse kinematics calculations through task space modeling, reduce computational complexity and hardware costs, and have a simple structure and are easy to implement. The error linearization of Lie group algebra is used to solve the Euler angle singularity problem, thereby improving trajectory tracking accuracy and stability. The geometric model predictive control (GMPC) design based on Lie group enhances anti-disturbance robustness and strictly satisfies constraints. The disturbance observer and weighted whole-body controller are integrated to increase the control frequency to above 800 Hz, achieving millimeter-level precision tracking and strong robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, a brief introduction will be given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0034] Figure 1 A schematic flow chart of a robot arm trajectory tracking method based on geometric model predictive control provided by an embodiment of the present invention;

[0035] Figure 2 A schematic structural diagram of a robot arm trajectory tracking device based on geometric model predictive control provided by an embodiment of the present invention;

[0036] Figure 3 A schematic diagram of the physical structure of an electronic device provided by an embodiment of the present invention;

[0037] Figure 4A schematic structural diagram of a robotic arm trajectory tracking system based on geometric model predictive control provided by an embodiment of the present invention;

[0038] Figure 5 A schematic diagram of the tracking effect of the end-arm posture trajectory provided by an embodiment of the present invention;

[0039] Figure 6 A schematic diagram of the posture error effect of the end-arm of the robot provided by an embodiment of the present invention;

[0040] Figure 7 A schematic diagram of the trajectory tracking error effect of a six-degree-of-freedom industrial robot arm provided by an embodiment of the present invention;

[0041] Figure 8 Schematic diagram of trajectory tracking posture of a six-degree-of-freedom industrial robot arm provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0042] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. In addition, the technical features in the various embodiments or single embodiments provided by the present invention can be combined with each other arbitrarily to form a feasible technical solution. This combination is not subject to the constraints of the sequence of steps and / or structural composition mode, but must be based on the ability of ordinary technicians in this field to implement. When the combination of technical solutions is mutually contradictory or cannot be implemented, it should be considered that the combination of such technical solutions does not exist and is not within the scope of protection required by the present invention. If there are step numbers in the following embodiments, they are only set for the convenience of explanation and description, and the order between the steps is not limited in any way. The execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art. To address technical issues such as Euler angle singularities, difficulty in constraint handling, and insufficient robustness to disturbances in traditional robot control, this paper innovatively combines Lie group geometry theory with tubular model predictive control, establishes a linearized dynamic model in the task space, and implements high-precision, high-frequency robust trajectory tracking control of the manipulator through Lie algebraic error linearization technology and robust invariant set construction. Based on this, the present invention provides a manipulator trajectory tracking method based on geometric model predictive control, see Figure 1The method includes: establishing and linearizing a manipulator dynamics model in a task space; defining a left-invariant tracking error based on Lie group theory, and linearizing the left-invariant tracking error in the tangent space of the Lie group to obtain an error dynamics equation; constructing a nominal geometric model predictive controller, establishing an augmented state space and constructing a cost function; constructing a tubular geometric model predictive controller, and using the minimum robust positive invariant set theory to solve a finite time domain discrete optimization problem; integrating a weighted whole-body controller, and performing predetermined frequency control on the manipulator trajectory tracking by solving a convex quadratic programming problem.

[0043] Based on the content of the above method embodiment, as an optional embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, wherein the robot arm dynamics model is established and linearized in the task space, including: using the special Euclidean group SE (3) manifold to represent the end effector posture, establishing a mapping relationship between the joint space and the task space spinor through the Jacobian matrix, defining the rate of change of the joint angle within the sampling interval, and linearizing the robot arm dynamics model in the task space into a linear non-homogeneous equation:

[0044]

[0045] u=τ

[0046] F t =J(q)M(q) -1

[0047] b t =J(q)M(q) -1 (wG(q))

[0048] Among them, H t is the task space inertia matrix; F t is the force transmission matrix; b t is the bias term; V t is the actual motion spinor; u is the control input; J(q) is the Jacobian matrix; q is the joint angle; is the Coriolis force and centrifugal force; M(q) is the inertia matrix; M(q) -1 is the inverse of the inertia matrix; J(q) -1 is the inverse of the Jacobian matrix; τ is the joint torque; w is the external disturbance; G(q) is the gravity; · is the sign of the first-order derivative with respect to time.

[0049] The traditional joint space dynamics model of the robotic arm can be described as:

[0050]

[0051] Where q is the joint angle, is the joint angular velocity, M(q) is the inertia matrix, is the Coriolis force and centrifugal force, G(q) is gravity, τ is the joint torque, and w is the disturbance. It can be seen that the above equations have the characteristics of multi-variable coupling and high nonlinearity, which easily leads to singularity problems and brings difficulties to real-time control. Therefore, the present invention avoids the computational burden of repeatedly solving inverse kinematics required for traditional joint space control by directly performing control design in the task space, and significantly improves the computational efficiency and numerical stability of the control algorithm. For the n-DOF manipulator, a dynamic model is established in the task space. First, the position and posture of the end effector are represented globally by the special Euclidean group SE(3):

[0052]

[0053] The rotation matrix R represents the posture of the end effector of the manipulator, and the position vector p represents its position in three-dimensional space. The motion rotation of the end effector in the body coordinate system is defined as Where (·) ∨ represents the antisymmetric matrix transformation, w b represents the angular velocity, v b Represents the linear velocity. Through the Jacobian matrix J b Establish the mapping relationship between joint space and task space rotation: Based on the reasonable engineering assumption that the joint angle q changes slowly within the sampling interval, the strongly nonlinear task space dynamics is linearized into a form that is convenient for controller design, and then the linearized expression of the dynamic equation of the task space is obtained as follows: As shown in the equation, the linear non-homogeneous task space dynamics equation is obtained, which makes it easier to design an efficient controller. At the same time, the influence of external disturbances and gravity is comprehensively considered, providing a mathematical basis for the robust design of the system.

[0054] Based on the content of the above method embodiment, as an optional embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, the left invariant tracking error is defined based on Lie group theory, and the left invariant tracking error is linearized in the tangent space of the Lie group to obtain the error dynamics equation, including:

[0055]

[0056]

[0057] Among them, t is the left invariant tracking error; g d,t is the expected trajectory; -1 is the inverse sign; g t is the actual trajectory; is the linearized spinor error expressed in Lie algebra; is the adjoint operator; ∧ is the symbol of the antisymmetric matrix; ω is the angular velocity of the end effector; v is the linear velocity of the end effector; V d,t is the desired motion spinor.

[0058] Traditional error definition methods often have coordinate system dependence and local validity problems, while the left-invariant error definition adopted in the present invention has the significant advantages of coordinate independence and global consistency.

[0059] Next, we derive the exact expression of error dynamics through the differential geometry theory on Lie groups. By taking the derivatives on both sides, we can derive the error dynamics equation: in Represents coordinate mapping, V t and V d,t denote the actual and desired motion spinors, respectively. The strong nonlinear characteristics of the error dynamics equation above will lead to a non-convex optimization problem, so it is linearized on the Lie group tangent space. Using the first-order approximation You can Convert to shown.

[0060] Based on the content of the above method embodiment, as an optional embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, wherein the construction of the nominal geometric model predictive controller includes:

[0061]

[0062] in, is the nominal geometric model predictive controller; t is the time; ΔT is the time difference; is the stage cost function; is the terminal cost function; It is the nominal state; is the nominal input; Q is the symbol of Q norm; R is the symbol of R norm; P is the symbol of P norm; To define symbols.

[0063] In order to handle tracking error and system dynamics simultaneously, the present invention designs an augmented state space representation: In order to facilitate controller design, the control output vector is further defined as The output includes the posture error and the rate of change of the error, providing the controller with richer feedback information. Based on this, a linear state space is established:

[0064]

[0065] in The relationship between output and input is:

[0066] y t =C t x t -d t

[0067]

[0068] At this point, the nominal controller design can be obtained as shown.

[0069] Based on the content of the above method embodiment, as an optional embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, wherein the tubular geometric model predictive controller is constructed, includes:

[0070]

[0071]

[0072] Among them, min is the minimum value symbol; u k is the actual input; is the cost function; N is the prediction time domain; k is the time step; is the nominal state; y k is the actual state; T is the matrix transpose symbol; Q is the state weight; is the nominal input; R is the input weight; is the terminal state; P is the terminal weight; is the state transfer matrix; is the control input matrix; C k is the output matrix; is the interference input matrix; A k is the state transfer matrix before coordinate transformation; B k h is the control input matrix before coordinate transformation; k Output matrix before coordinate transformation; d k is the feedforward compensation matrix; is the state constraint set; Subtraction for the Minkowski set; is a robust invariant set; is the addition of the Minkowski set; Ω tube is the terminal set; U tube To tighten the input constraint set; K k is the feedback coefficient; j is the power; s is the iteration step; W k is the disturbance invariant set; K is the state feedback control gain matrix.

[0073] In order to ensure that the system strictly satisfies the constraints under bounded disturbances, the theory of minimum robust positive invariant sets is introduced. Traditional model predictive control often has difficulty in dealing with the influence of system uncertainty and external disturbances on constraint satisfaction. However, the present invention theoretically guarantees the strict satisfaction of constraints by constructing robust invariant sets. At each control moment, the finite time domain discrete optimization problem is solved as follows The terminal weights P and the state feedback control gain matrix K are calculated by solving the discrete-time algebraic Riccati equation:

[0074] P=A T PA-(A T PB(R+B T PB) -1 B T PA)+Q

[0075] K=(R+B T PB) -1 B T PA

[0076] W k is the disturbance invariant set, which is observed by the disturbance observer and input to the geometric model predictive controller.

[0077] Based on the content of the above method embodiment, as an optional embodiment, the robot arm trajectory tracking method based on geometric model predictive control provided in the embodiment of the present invention, the integrated weighted whole body controller includes:

[0078]

[0079] Ax=b,low≤Ax≤up

[0080]

[0081] Among them, w i is the i-th weight coefficient; |||| is the norm symbol; A i is the coefficient matrix of the i-th task; b i is the constant of the i-th task; A is the coefficient matrix; x is the decision variable; b is a constant; low is the lower bound of the constraint; pos is the position; ori is the initial value; up is the upper bound of the constraint; ·· is the sign of the second-order derivative of the joint angle with respect to time.

[0082] To address the limited control frequency of pipeline GMPC due to the need to solve robust invariant sets online, this paper innovatively integrates a weighted whole-body controller as a low-level high-frequency controller. This two-layer control architecture effectively resolves the conflict between control accuracy and computation frequency, increasing the system's actual control frequency to over 800Hz, meeting the stringent requirements of high-precision force control and fast dynamic response.

[0083] In order to achieve multi-task coordinated control, a weighted multi-objective optimization framework is designed, which mainly includes two main control objectives: first, to make the output of the whole body controller as close as possible to the torque output of the GMPC controller; second, to minimize the tracking error of the end of the manipulator. The core of the whole body controller is to solve the structured convex quadratic programming problem such as This construction ensures the strong convexity and numerical stability of the objective function, enabling the optimization problem to be solved in milliseconds.

[0084] The robotic arm trajectory tracking method based on geometric model predictive control provided by the embodiment of the present invention avoids inverse kinematics calculations through task space modeling, reduces computational complexity and hardware costs, and has a simple structure and is easy to implement. It uses Lie group algebra error linearization to solve the Euler angle singularity problem and improve trajectory tracking accuracy and stability. The geometric model predictive control (GMPC) design based on Lie groups enhances anti-disturbance robustness and strictly satisfies constraints. The integrated disturbance observer and weighted whole-body controller increase the control frequency to above 800 Hz, achieving millimeter-level precision tracking and strong robustness.

[0085] The embodiment of the present invention provides a robot arm trajectory tracking system (i.e. hardware structure) based on geometric model predictive control, see Figure 4 The system includes: a disturbance observer for obtaining observation values ​​output by the robotic arm and the whole-body controller; a robotic arm for executing control results; a geometric model predictive controller for enhancing system robustness; and a whole-body controller for loading a corresponding program to implement the robotic arm trajectory tracking method based on geometric model predictive control as described in any of the aforementioned method embodiments.

[0086] A 6-DOF robotic arm simulation experiment was conducted using Matlab. Ten initial end-point poses were randomly generated under both undisturbed and disturbed conditions, and the arm was forced to follow a spiral trajectory. Figure 5 and Figure 6 The figures represent the trajectory tracking and pose error of the end-arm position, respectively. The end-arm position and pose converge quickly, remaining within safety constraints. A 6-DOF industrial robotic arm was experimentally validated for fixed-point and spiral trajectory tracking under disturbances. The proposed algorithm was executed on a NUC running the Ubuntu 20.04 Linux operating system. The software relies solely on open-source modules such as OSQP, qpOASES, and pinocchio.

[0087] The tracking error effect of the end-arm posture in the experiment can be found in Figure 7 and Figure 8Experimental results show that: in terms of tracking accuracy, the method of the present invention achieves millimeter-level accuracy in position and posture tracking, and can realize end-point tracking control of any initial point; in terms of control frequency, the nominal GMPC controller control frequency can reach above 200 Hz, and the accelerated control frequency can reach above 800 Hz, effectively ensuring high control performance; in terms of anti-disturbance control, the method of the present invention shows strong robustness and stability in the presence of random disturbances such as friction and load, and joint constraints and safety limits are strictly guaranteed.

[0088] The implementation basis of each embodiment of the present invention is to implement programmed processing through a device with processor functions. Therefore, in engineering practice, the technical solutions and functions of each embodiment of the present invention can be encapsulated into various modules (i.e., software). Based on this reality, on the basis of the above embodiments, an embodiment of the present invention provides a robot arm trajectory tracking device based on geometric model predictive control, which is used to execute the robot arm trajectory tracking method based on geometric model predictive control in the above method embodiment. See Figure 2 The device includes: a first main module, which is used to establish and linearize the manipulator dynamics model in the task space; a second main module, which is used to define the left-invariant tracking error based on the Lie group theory, and linearize the left-invariant tracking error in the tangent space of the Lie group to obtain the error dynamics equation; a third main module, which is used to build a nominal geometric model predictive controller, establish an augmented state space and construct a cost function; a fourth main module, which is used to build a tubular geometric model predictive controller, and use the minimum robust positive invariant set theory to solve the finite time domain discrete optimization problem; a fifth main module, which is used to implement an integrated weighted whole-body controller, and perform predetermined frequency control on the manipulator trajectory tracking by solving a convex quadratic programming problem.

[0089] The robot arm trajectory tracking device based on geometric model predictive control provided by the embodiment of the present invention adopts Figure 2 Several modules in it avoid inverse kinematics calculations through task space modeling, reducing computational complexity and hardware costs, with a simple structure and easy implementation; Lie group algebra error linearization is used to solve the Euler angle singularity problem and improve trajectory tracking accuracy and stability; the geometric model predictive control GMPC design based on Lie group enhances anti-disturbance robustness and strictly satisfies constraints; the integrated disturbance observer and weighted whole-body controller increase the control frequency to above 800 Hz, achieving millimeter-level precision tracking and strong robustness.

[0090] It should be noted that the device in the device embodiment provided by the present invention can be used to implement the method in the above-mentioned method embodiment as well as the method in other method embodiments provided by the present invention. The only difference is that the corresponding functional module (i.e., software) is set. The principle is basically the same as the principle of the above-mentioned device embodiment provided by the present invention. As long as those skilled in the art refer to the specific technical solutions in other method embodiments on the basis of the above-mentioned device embodiment, obtain the corresponding technical means and the technical solutions composed of these technical means by combining technical features, and ensure the practicality of the technical solutions, they can improve the device in the above-mentioned device embodiment to obtain the corresponding device class embodiment (i.e., software) for implementing the methods in other method class embodiments. For example:

[0091] Based on the content of the above-mentioned device embodiment, as an optional embodiment, the robot arm trajectory tracking device based on geometric model predictive control provided in the embodiment of the present invention further includes: a first submodule for realizing the establishment and linearization of the robot arm dynamics model in the task space, including: using the special Euclidean group SE(3) manifold to represent the end effector posture, establishing the mapping relationship between the joint space and the task space spinor through the Jacobian matrix, defining the rate of change of the joint angle within the sampling interval, and linearizing the robot arm dynamics model in the task space into a linear non-homogeneous equation:

[0092]

[0093] u=τ

[0094] F t =J(q)M(q) -1

[0095] b t =J(q)M(q) -1 (wG(q))

[0096] Among them, H t is the task space inertia matrix; F t is the force transmission matrix; b t is the bias term; V t is the actual motion spinor; u is the controller; J(q) is the Jacobian matrix; q is the joint angle; is the Coriolis force and centrifugal force; M(q) is the inertia matrix; M(q) -1 is the inverse of the inertia matrix; J(q) -1 is the inverse of the Jacobian matrix; τ is the joint torque; w is the external disturbance; G(q) is the gravity; · is the sign of the first-order derivative with respect to time.

[0097] Based on the content of the above device embodiment, as an optional embodiment, the robot arm trajectory tracking device based on geometric model predictive control provided in the embodiment of the present invention further includes: a second submodule for implementing the definition of the left invariant tracking error based on Lie group theory, and linearizing the left invariant tracking error in the tangent space of the Lie group to obtain an error dynamics equation, including:

[0098]

[0099] Among them, t is the left invariant tracking error; g d,t is the expected trajectory; -1 is the inverse sign; g t is the actual trajectory; is the linearized spinor error expressed in Lie algebra; is the adjoint operator; ∧ is the symbol of the antisymmetric matrix; ω is the angular velocity of the end effector; v is the linear velocity of the end effector; V d,t is the desired motion spinor.

[0100] Based on the content of the above device embodiment, as an optional embodiment, the robot arm trajectory tracking device based on geometric model predictive control provided in the embodiment of the present invention further includes: a third submodule for implementing the construction of the nominal geometric model predictive controller, including:

[0101]

[0102] in, is the nominal geometric model predictive controller; t is the time; ΔT is the time difference; is the stage cost function; is the terminal cost function; It is the nominal state; is the nominal input; Q is the symbol of Q norm; R is the symbol of R norm; P is the symbol of P norm; To define symbols.

[0103] Based on the content of the above device embodiment, as an optional embodiment, the robot arm trajectory tracking device based on geometric model predictive control provided in the embodiment of the present invention further includes: a fourth submodule for implementing the construction of the tubular geometric model predictive controller, including:

[0104]

[0105] Among them, min is the minimum value symbol; u k is the actual input; is the cost function; N is the prediction time domain; k is the time step; is the nominal state; y k is the actual state; T is the matrix transpose symbol; Q is the state weight; is the nominal input; R is the input weight; is the terminal state; P is the terminal weight; is the state transfer matrix; is the control input matrix; C k is the output matrix; is the interference input matrix; A k is the state transfer matrix before coordinate transformation; B k h is the control input matrix before coordinate transformation; k Output matrix before coordinate transformation; d k is the feedforward compensation matrix; is the state constraint set; Subtraction for the Minkowski set; is a robust invariant set; is the addition of the Minkowski set; Ω tube is the terminal set; U tube To tighten the input constraint set; K k is the feedback coefficient; j is the power; s is the iteration step; W k is the disturbance invariant set; K is the state feedback control gain matrix.

[0106] Based on the content of the above device embodiment, as an optional embodiment, the robot arm trajectory tracking device based on geometric model predictive control provided in the embodiment of the present invention further includes: a fifth submodule for implementing the integrated weighted whole-body controller, including:

[0107]

[0108] Ax=b,low≤Ax≤up

[0109]

[0110] Among them, w i is the i-th weight coefficient; |||| is the norm symbol; A i is the coefficient matrix of the i-th task; b i is the constant of the i-th task; A is the coefficient matrix; x is the decision variable; b is a constant; low is the lower bound of the constraint; pos is the end position; ori is the end posture; up is the upper bound of the constraint; ·· is the sign of the second-order derivative of the joint angle with respect to time.

[0111] The method of the embodiment of the present invention is implemented by electronic devices, so it is necessary to introduce the relevant electronic devices. Based on this purpose, the embodiment of the present invention provides an electronic device, such as Figure 3As shown, the electronic device includes: at least one processor, a communications interface, at least one memory, and a communications bus, wherein the at least one processor, the communications interface, and the at least one memory communicate with each other via the communications bus. The at least one processor can call logic instructions in the at least one memory to execute all or part of the steps of the methods provided in the aforementioned method embodiments.

[0112] In addition, the logic instructions in the at least one memory mentioned above can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each method embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0113] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0114] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course can also be implemented by hardware. Based on this understanding, the essence of the above technical solution or the part that contributes to the existing technology can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiment.

[0115] The flowcharts and block diagrams in the accompanying drawings show the possible architectures, functions and operations of the systems, methods and computer program products according to multiple embodiments of the present invention. Based on this understanding, each box in the flowchart or block diagram can represent a module, program segment or part of the code, and the module, program segment or part of the code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, or sometimes in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart, can be implemented with a dedicated hardware-based system that performs the specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0116] It should be noted that the terms "include", "comprising" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device that includes a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "include..." do not exclude the presence of other identical elements in the process, method, article or device that includes the elements. Any "predetermined threshold", "preset threshold" or similar expressions that do not indicate a specific value can be determined by a person of ordinary skill in the art through simple experiments or corresponding debugging.

[0117] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A robot arm trajectory tracking method based on geometric model predictive control, characterized in that: include: Establish and linearize the manipulator dynamics model in the task space; define the left-invariant tracking error based on Lie group theory, and linearize the left-invariant tracking error in the tangent space of the Lie group to obtain the error dynamics equation; Construct a nominal geometric model predictive controller, establish an augmented state space and construct a cost function; A tubular geometric model predictive controller is constructed, and the minimum robust positive invariant set theory is used to solve the discrete optimization problem in the finite time domain. A weighted whole-body controller is integrated to perform predetermined frequency control of the robot arm trajectory tracking by solving a convex quadratic programming problem.

2. The robot arm trajectory tracking method based on geometric model predictive control according to claim 1, characterized in that: The method of establishing and linearizing the dynamic model of the manipulator in the task space includes: using a special Euclidean group SE(3) manifold to represent the end effector posture, establishing a mapping relationship between the spinors in the joint space and the task space through the Jacobian matrix, defining the rate of change of the joint angle within the sampling interval, and linearizing the dynamic model of the manipulator in the task space into a linear non-homogeneous equation: u=τ F t =J(q)M(q) -1 b t =J(q)M(q) -1 (w-G(q)) Among them, H t is the task space inertia matrix; F t is the force transmission matrix; b t is the bias term; V t is the actual motion spinor; u is the controller; J(q) is the Jacobian matrix; q is the joint angle; is the Coriolis force and centrifugal force; M(q) is the inertia matrix; M(q) -1 is the inverse of the inertia matrix; J(q) -1 is the inverse of the Jacobian matrix; τ is the joint torque; w is the external disturbance; G(q) is the gravity; · is the sign of the first-order derivative with respect to time.

3. The robot arm trajectory tracking method based on geometric model predictive control according to claim 2, characterized in that: The left-invariant tracking error is defined based on the Lie group theory, and the left-invariant tracking error is linearized in the tangent space of the Lie group to obtain the error dynamics equation, including: Among them, t is the left invariant tracking error; g d,t is the expected trajectory; -1 is the inverse sign; g t is the actual trajectory; is the linearized spinor error expressed in Lie algebra; is the adjoint operator; ∧ is the symbol of the antisymmetric matrix; ω is the angular velocity of the end effector; v is the linear velocity of the end effector; V d,t is the desired motion spinor.

4. The robot arm trajectory tracking method based on geometric model predictive control according to claim 3, characterized in that: The constructing of the nominal geometric model predictive controller includes: in, is the nominal geometric model predictive controller; t is the time; ΔT is the time difference; is the stage cost function; is the terminal cost function; It is the nominal state; is the nominal input; Q is the symbol of Q norm; R is the symbol of R norm; P is the symbol of P norm; To define symbols.

5. The robot arm trajectory tracking method based on geometric model predictive control according to claim 4, characterized in that: The construction of the tubular geometric model predictive controller includes: k=0,1,...,N-1 Among them, min is the minimum value symbol; u k is the actual input; is the cost function; N is the prediction time domain; k is the time step; is the nominal state; y k is the actual state; T is the matrix transpose symbol; Q is the state weight; is the nominal input; R is the input weight; is the terminal state; P is the terminal weight; is the state transfer matrix; is the control input matrix; C k is the output matrix; is the interference input matrix; A k is the state transfer matrix before coordinate transformation; B k h is the control input matrix before coordinate transformation; k Output matrix before coordinate transformation; d k is the feedforward compensation matrix; y is the state constraint set; Subtraction for the Minkowski set; is a robust invariant set; is the addition of the Minkowski set; Ω tube is the terminal set; U tube To tighten the input constraint set; K k is the feedback coefficient; j is the power; s is the number of iteration steps; W k is the disturbance invariant set; K is the state feedback control gain matrix.

6. The robot arm trajectory tracking method based on geometric model predictive control according to claim 5, characterized in that: The integrated weighted whole-body controller comprises: Ax=b,low≤Ax≤up Among them, w i is the i-th weight coefficient; || || is the norm symbol; A i is the coefficient matrix of the i-th task; b i is the constant of the i-th task; A is the coefficient matrix; x is the decision variable; b is a constant; low is the lower bound of the constraint; pos is the position; ori is the posture; up is the upper bound of the constraint; ·· is the sign of the second-order derivative of the joint angle with respect to time.

7. A robot arm trajectory tracking system based on geometric model predictive control, characterized in that: include: A disturbance observer is used to obtain observations of the outputs of the manipulator and the whole-body controller; A robotic arm, used to execute control results; Geometric model predictive controller to enhance system robustness; A whole-body controller, used for loading a corresponding program to implement the robot arm trajectory tracking method based on geometric model predictive control as claimed in any one of claims 1 to 6.

8. A robot arm trajectory tracking device based on geometric model predictive control, characterized in that: include: The first main module is used to establish and linearize the dynamic model of the manipulator in the task space; The second main module is used to define the left-invariant tracking error based on Lie group theory, and linearize the left-invariant tracking error in the tangent space of the Lie group to obtain the error dynamics equation; the third main module is used to build a nominal geometric model predictive controller, establish an augmented state space and construct a cost function; the fourth main module is used to build a tubular geometric model predictive controller, and use the minimum robust positive invariant set theory to solve the finite time domain discrete optimization problem; the fifth main module is used to implement an integrated weighted whole-body controller, and perform predetermined frequency control on the robot arm trajectory tracking by solving a convex quadratic programming problem.

9. An electronic device, characterized in that: include: At least one processor, at least one memory and a communication interface; wherein, The processor, memory and communication interface communicate with each other; The memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the method according to any one of claims 1 to 6.

10. A non-transitory computer-readable storage medium, characterized in that The non-transitory computer-readable storage medium stores computer instructions, which enable a computer to execute the method of any one of claims 1 to 6.

Images