Robot variable impedance control method and system based on model predictive control and control equipment

Abstract

The invention provides a robot variable impedance control method and system based on model predictive control and control equipment, and the method comprises the steps: constructing an iteration vector based on a current state vector and a control vector, and executing the iteration of the iteration vector under a plurality of set constraint conditions through employing an optimization model based on a non-linear objective function and the iteration vector, the plurality of constraint conditions comprise impedance parameters, upper and lower bound constraints of rigidity parameters, change rate constraints and passivity constraints. And extracting a control vector in the iteration vector, and obtaining an impedance matrix and a stiffness matrix based on the extracted control vector. And inputting the impedance matrix and the stiffness matrix into an impedance controller, and predicting to obtain a torque for executing robot control. According to the scheme, follow-up behaviors of the system can be predicted in a rolling mode in each control period, control input is optimized under the constraint condition, a systematic optimization strategy is provided for parameter adjustment, multi-target performance indexes and hardware constraints can be considered, and control robustness is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, in particular to a robot variable impedance control method, system and control device based on model predictive control. BACKGROUND

[0002] Impedance control technology is a classic robot compliant control method, and its core is to establish a fixed mass-damping-stiffness model for the interaction between the robot end effector and the external environment. Through the model, the system can realize the dynamic mapping between force and displacement during contact, so as to effectively control the change of contact force while meeting the position tracking. Due to its simple structure and convenient implementation, constant impedance control is widely used in polishing, assembly, rehabilitation and other force contact tasks, which can improve the stability and safety of robot and environment or human interaction to a certain extent.

[0003] However, since the impedance parameters remain constant during control, the system cannot dynamically adjust the compliance according to the changes of the external environment, resulting in insufficient adaptability, especially in tasks with significant uncertainty or variable contact conditions. In the interaction process between the robot and the environment or human, the control system not only needs to have accurate position tracking capability, but also must show good compliance and adaptability. The traditional impedance control effectively adjusts the dynamic behavior of the robot end effector by introducing a mass-damping-spring model, but its fixed parameter setting is difficult to adapt to complex and variable interactive environment. Therefore, variable impedance control (VIC) emerges as the times require, which dynamically balances between rigidity and compliance by adjusting impedance parameters in real time. However, the selection of variable impedance parameters often depends on experience, lacks systematic adjustment mechanism, and is difficult to meet the requirements of interaction safety and task execution performance at the same time.

[0004] In the prior art, the given reference impedance parameters are tracked along the trajectory, and the reference value of the impedance parameters needs to be given in advance. In addition, the setting of the parameters highly depends on artificial experience, lacks unified adjustment criteria, and often needs to balance between safety and performance, which limits its popularization and application in complex environments. Moreover, the stability problem caused by variable impedance is not considered in the control process. SUMMARY

[0005] The purpose of the embodiments of the present application is to provide a robot variable impedance control method, system and control device based on model predictive control, which can balance multiple target performance indicators and hardware constraints, and improve the robustness of control.

[0006] In a first aspect, the present application provides a robot variable impedance control method based on model predictive control, the method comprising: Acquire a current state vector and a control vector during the robot control process, and construct an iteration vector based on the state vector and the control vector; Using the optimization model based on the constructed nonlinear objective function and the iteration vector, the iteration vector is iterated under the constraints of multiple set constraints until preset requirements are met, the multiple constraints including upper and lower bound constraints of impedance parameters, stiffness parameters, rate of change constraints, and passivity constraints; extracting a control vector from the iterative vector, and obtaining an impedance parameter vector and a stiffness parameter vector based on the extracted control vector; Expanding the impedance parameter vector and the stiffness parameter vector to obtain an impedance matrix and a stiffness matrix; The impedance matrix and the stiffness matrix are input into the impedance controller to predict the torque for executing robot control.

[0007] In an optional embodiment, the step of obtaining the current state vector and control vector during the robot control process includes: Obtain the current position error, impedance parameters, stiffness parameters and external forces during robot control; constructing a state vector based on the position error, impedance parameter, stiffness parameter and external force; A control vector is constructed based on the impedance parameter and the stiffness parameter.

[0008] In an optional embodiment, the nonlinear objective function is constructed in the following manner: Based on the Cartesian pose of the robot end at the initial planning moment, the Cartesian inertia matrix is ​​obtained; Constructing a continuous-time state equation based on the control vector and the Cartesian inertia matrix; discretizing the continuous-time state equation to obtain a discrete-time state equation; Under the discrete-time state equation and multiple set constraints, a nonlinear objective function constructed by a stage cost function and a terminal cost function is constructed based on the state vector and the control vector.

[0009] In an optional embodiment, the stage cost function is constructed in the following manner: constructing a reference stiffness function that varies with position error, and obtaining a stiffness adjustment rate based on the reference stiffness function; constructing an impedance coefficient based on the stiffness adjustment rate; Obtaining a state reference item according to the stiffness adjustment rate, the impedance coefficient, and the external force; A stage cost function about the control vector and the state vector is constructed based on the state reference item, the control vector and the state vector.

[0010] In an optional embodiment, the passivity constraint in the plurality of constraints is constructed in the following manner: The total energy of the system is calculated based on the position error, the differential of the position error, the inertia matrix and the stiffness matrix; Derivative the total energy of the system to obtain a total energy change; Obtaining an external force input power based on a differential of the position error and the external force; A passivity constraint is constructed by combining the total energy change and the external force input power, so that the total energy change is smaller than the external force input power.

[0011] In an optional embodiment, the step of utilizing the optimization model based on the constructed nonlinear objective function and the iteration vector, and executing iteration of the iteration vector under the constraints of a plurality of set constraints until preset requirements are met, includes: Performing quadratic programming on the iteration vector using an optimization model to obtain a variable increment corresponding to the iteration vector; Constructing constraints and optimization subfunctions for the variable increments; The optimization sub-function is iterated under the constraint of the variable increment until the absolute value of the variable increment is less than a preset threshold, and it is determined that the preset requirement is met.

[0012] In an optional embodiment, the step of constructing the constraint and optimization sub-function of the variable increment includes: Calculating the first-order derivative gradient and the second-order derivative Hazen matrix of the constructed nonlinear objective function with respect to the iteration vector; Differentiating the state vector and the control vector in the iteration vector respectively, and differentiating the constraint function constructed by the set multiple constraint conditions to obtain an iteration coefficient; Constraints on variable increments and optimization subfunctions are obtained based on the iteration coefficient, the first-order derivative gradient and the second-order derivative Hazen matrix.

[0013] In an optional embodiment, the step of obtaining the impedance parameter vector and the stiffness parameter vector based on the extracted control vector includes: The elements in the extracted control vector are integrated respectively to obtain the impedance parameter vector and the stiffness parameter vector.

[0014] In a second aspect, the present invention provides a robot variable impedance control system based on model predictive control, the system comprising: An acquisition module is used to obtain the current state vector and control vector during the robot control process, and construct an iteration vector based on the state vector and control vector; An iteration module is configured to perform iteration of the iteration vector under a plurality of constraint conditions, including upper and lower bound constraints of impedance parameters and stiffness parameters, a variation rate constraint, and a passivity constraint, based on the constructed nonlinear objective function and the iteration vector by using an optimization model until a preset requirement is met. An extraction module is configured to extract a control vector from the iteration vector, and obtain an impedance parameter vector and a stiffness parameter vector based on the extracted control vector. An expansion module is configured to expand the impedance parameter vector and the stiffness parameter vector to obtain an impedance matrix and a stiffness matrix. A predictive control module is configured to input the impedance matrix and the stiffness matrix into an impedance controller to predict a torque for executing robot control.

[0015] In a third aspect, the present application provides a control device, which comprises one or more storage media and one or more processors in communication with the storage media, and the one or more storage media store machine executable instructions executable by the processor, and when the control device is running, the processor executes the machine executable instructions to perform the method in any one of the foregoing embodiments.

[0016] The present application provides a robot variable impedance control method, system and control device based on model predictive control, constructs an iteration vector based on a current state vector and a control vector, performs iteration of the iteration vector under a plurality of constraint conditions, including upper and lower bound constraints of impedance parameters and stiffness parameters, a variation rate constraint, and a passivity constraint, based on a nonlinear objective function and the iteration vector by using an optimization model until a preset requirement is met. A control vector is extracted from the iteration vector, and an impedance parameter vector and a stiffness parameter vector are obtained based on the extracted control vector. An impedance matrix and a stiffness matrix are expanded based on the impedance parameter vector and the stiffness parameter vector. The impedance matrix and the stiffness matrix are input into an impedance controller to predict a torque for executing robot control.

[0017] In the present application, the model predictive method is adopted, which can predict the subsequent behavior of the system in each control cycle and optimize the control input under the constraint conditions, not only providing a systematic optimization strategy for parameter adjustment, but also taking into account the multi-objective performance indicators and hardware constraints to improve the robustness of the control. BRIEF DESCRIPTION OF DRAWINGS

[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the embodiments of the present application. It should be understood that the following drawings only show some embodiments of the present application, and therefore should not be regarded as a limitation on the scope, and for those skilled in the art, other related drawings can also be obtained without creative labor.

[0019] Figure 1 A flow chart of a robot variable impedance control method based on model predictive control provided by an embodiment of the present application; Figure 2 A schematic diagram of a polishing simulation scene constructed in simulation software in an embodiment of the present application; Figure 3 A curve schematic diagram of normal contact force changing with time in an embodiment of the present application; Figure 4 A displacement curve schematic diagram under low impedance test in an embodiment of the present application; Figure 5 A displacement curve schematic diagram under high impedance test in an embodiment of the present application; Figure 6 A displacement curve schematic diagram under variable impedance test in an embodiment of the present application; Figure 7 A function module block diagram of a robot variable impedance control system based on model predictive control provided by an embodiment of the present application; Figure 8 A structure block diagram of a control device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be described below with reference to the accompanying drawings.

[0021] Please refer to Figure 1 A robot variable impedance control method based on model predictive control provided by an embodiment of the present application, which can be executed by a robot variable impedance control system based on model predictive control. The robot variable impedance control system based on model predictive control can be realized by software and / or hardware, and can be configured in a control device. The control device can be a computer device, a server, etc., for example, a server in a back-end control platform, etc. The detailed steps of the robot variable impedance control method based on model predictive control are introduced as follows.

[0022] S11, obtaining a current state vector and a control vector in a robot control process, and constructing an iteration vector based on the state vector and the control vector.

[0023] S12, performing iteration of the iteration vector under the constraint of a plurality of set constraints by using an optimization model based on a constructed nonlinear objective function and the iteration vector, until a preset requirement is met.

[0024] The plurality of constraints include upper and lower bound constraints of impedance parameters and stiffness parameters, variation rate constraints, and passivity constraints.

[0025] S13, extract the control vector in the iteration vector, obtain the impedance parameter vector and the stiffness parameter vector based on the extracted control vector.

[0026] S14, expand the impedance matrix and the stiffness matrix according to the impedance parameter vector and the stiffness parameter vector.

[0027] S15, input the impedance matrix and the stiffness matrix into the impedance controller, and predict the torque for executing robot control.

[0028] The technical scheme provided by the embodiment is realized under the Cartesian impedance control framework, and the calculation formula of the Cartesian impedance control rate is as follows:

[0029] Among them, is the control torque of the input mechanical arm, is the gravity term, is the joint angle, is the Jacobian matrix, is the inertia matrix in the Cartesian space, is the Coriolis matrix in the Cartesian space, is the impedance matrix, is the stiffness matrix, is the pose deviation, is the desired trajectory.

[0030] Based on the above control rate calculation formula, the following dynamics relationship is established:

[0031] Among them is a six-dimensional vector composed of the force and torque acting on the end of the mechanical arm.

[0032] The above dynamics relationship equation is rearranged to represent the acceleration term, and the following expression is obtained:

[0033] In order to facilitate subsequent modeling and optimization of impedance parameters as system state variables, in view of the fact that the impedance matrix and the stiffness matrix have been designed as diagonal matrices, the impedance parameter vector and the stiffness parameter vector composed of the diagonal elements thereof can be used to represent the above two matrices respectively, as follows:

[0034] Among them:

[0035] On this basis, the system state variable is uniformly constructed as a state vector , and the control vector is represented as The above step of obtaining the current state vector and control vector in the robot control process can be implemented in the following manner: Obtaining the current position error, impedance parameter, stiffness parameter and external force in the robot control process; constructing a state vector based on the position error, impedance parameter, stiffness parameter and external force; constructing a control vector based on the impedance parameter and stiffness parameter.

[0036] Specifically, based on the position error, impedance parameter, stiffness parameter and external force, the state vector shown below is constructed :

[0037] In addition, based on the impedance parameter and stiffness parameter, the control vector shown below is constructed :

[0038] In this embodiment, the state vector and the control vector are spliced together to form an iteration vector, which can be represented as .

[0039] In this embodiment, a nonlinear model predictive control (NMPC) method is used to achieve high-precision control. In this control method, multiple constraint conditions are considered, and the NMPC problem is converted into a nonlinear rule (NLP) problem in a finite time domain. For the NLP problem, common solutions include algorithms such as the Lagrange multiplier method, the trust region method, and the penalty function method, to obtain the optimal solution of the NLP problem. However, related theories and algorithms still need to be improved in practical applications. SQP based on the Newton-Lagrange method is one of the optimal algorithms for solving nonlinear programming problems. In this embodiment, for the case where the nonlinear objective function and complex constraint conditions are included in the optimization model, a sequential quadratic programming method is used for solution. This method solves an approximate second-order programming sub-problem in each iteration to approximate the optimal solution of the original nonlinear programming problem.

[0040] In the above step of performing iteration of the iteration vector under the constraint of the set multiple constraint conditions based on the constructed nonlinear objective function and iteration vector using the optimization model, the nonlinear objective function is constructed in the following manner: Based on the robot end Cartesian pose at the initial planning time, a Cartesian inertia matrix is obtained; a continuous time state equation is constructed according to the control vector and the Cartesian inertia matrix; the continuous time state equation is discretized to obtain a discrete time state equation; based on the state vector and the control vector, a nonlinear objective function constructed by a stage cost function and a terminal cost function is constructed under the discrete time state equation and a plurality of constraint conditions.

[0041] In this embodiment, in order to simplify the calculation complexity in model predictive control, it is assumed that the Cartesian inertia matrix and the external force remain unchanged in the entire prediction time domain, and are respectively approximated to the values thereof at the planning start time . The specific representation is as shown below:

[0042] wherein, is the robot end Cartesian pose at the initial planning time .

[0043] According to the control vector and the Cartesian inertia matrix, a continuous time state equation as shown below is constructed:

[0044] For ease of analysis, it is simply denoted as:

[0045] Sampling a fixed step size , the continuous time state equation is discretized to obtain the discrete time state equation of the system as:

[0046] Based on the model predictive control (MPC) framework, a nonlinear objective function with step prediction ability is constructed as follows:

[0047] wherein, is the sequence of optimization variables, is the stage cost function, is the terminal cost function.

[0048] The design of the objective function needs to consider that in the actual task execution process, the system often faces a complex interactive environment, including external disturbance, contact uncertainty and the deviation between the expected trajectory and the actual trajectory. Therefore, both compliance and control accuracy need to be reflected. Specifically, the stiffness and impedance should be adjusted in real time according to the task state, especially the size and position deviation of the external force.

[0049] The design of the objective function should not only consider system stability and minimization of trajectory tracking error, but also incorporate impedance parameter adjustment behavior into the optimization framework, so that the controller has higher robustness and response sensitivity while meeting dynamic performance requirements.

[0050] For the adjustment requirement of stiffness, the ideal adjustment mechanism should have the following characteristics: when the deviation is small, the stiffness should not have excessive response to maintain the flexibility and safety of the system and avoid rigid response to small disturbances; and when the deviation gradually increases, the stiffness should be moderately increased to improve the disturbance rejection capability of the system and avoid error accumulation and task failure. Such design ensures the dynamic trade-off between flexibility and precision, that is, the system mainly interacts in a flexible manner in the normal state, and in the scene where the deviation from the target is large or the contact rigidity is strong, the control precision and system stability are improved.

[0051] In impedance control, the dynamic characteristics of the environmental interaction force can be characterized by the motion deviation.

[0052] The nonlinear objective function constructed above includes a terminal cost function and a stage cost function, wherein the terminal cost function can be constructed by using the currently common construction method, and the stage cost function can be constructed by the following method: A reference stiffness function varying with the position error is constructed, a stiffness adjustment rate is obtained based on the reference stiffness function, impedance coefficients are constructed based on the stiffness adjustment rate, a state reference term is obtained according to the stiffness adjustment rate, the impedance coefficients and the external force, and a stage cost function about the control vector and the state vector is constructed based on the state reference term, the control vector and the state vector.

[0053] In this embodiment, in order to realize the autonomous adjustment of the stiffness parameter, a reference stiffness function varying with the position error is constructed , which satisfies the following characteristics: Strictly increasing: , which ensures that the stiffness increases when the deviation increases, and improves the disturbance rejection capability.

[0054] Convex function characteristics: , which ensures that the stiffness growth rate increases with the increase of the error, and adapts to the sudden disturbance.

[0055] The natural exponential function is used to construct the stiffness adjustment rate:

[0056] Wherein, is a reference stiffness coefficient, is a stiffness growth rate coefficient.

[0057] The stiffness adjustment also needs to maintain good damping characteristics, further design impedance coefficient to prevent oscillation, .

[0058] Combined with the stiffness adjustment rate, impedance coefficient and external force, the state reference term is constructed as shown below:

[0059] where, represents the six-dimensional pose error.

[0060] , represents the function operation on each element of the vector, and the output is a vector of corresponding dimension.

[0061] On this basis, based on the state reference term, control vector and state vector, the stage cost function about control vector and state vector is constructed:

[0062] where, is the weight matrix of the state vector, which is used to measure the cost of the system deviating from the desired state. is the weight matrix of the control input, which suppresses the size of the input to prevent the state from changing dramatically. The desired state is designed as the differential of the pose deviation, which is a zero vector, i.e. when the system tends to be stable, the components no longer change, in addition, the expected values of damping and stiffness are set as the dynamic adjustment goals of damping and stiffness in the task.

[0063] In order to ensure the safety and stability of the system in the parameter adjustment process, the design of the constraint condition mainly focuses on the following two aspects: Upper and lower bound constraints of stiffness and damping and their change rates: by setting the upper and lower limits of stiffness and damping, it is avoided that the control performance is insufficient due to too small numerical value, or rigid collision, system excitation is too strong and other problems caused by too large value. The constraint ensures that the parameter adjustment is within the range of physical feasibility and system stability, and improves the robustness and execution reliability of the control.

[0064] Passivity constraint to ensure stability: in order to avoid the energy non-conservation or unstable behavior caused by the change of impedance parameters, the constraint mechanism based on passivity is introduced. Specifically, by constructing the passivity function and ensuring that the impedance system satisfies the energy non-increasing (passive) or strictly passive condition at any time, the direction and rate of parameter adjustment are limited, so that the system is always in a controllable domain with stable energy. The constraint not only can effectively suppress the amplification effect of external disturbance, but also can enhance the stability and safety in the process of interaction with the environment.

[0065] Specifically, the constraint design of variable impedance control is as follows: The constraint design of control is as follows:

[0066] In the variable impedance control task, the stiffness and damping parameters need to be dynamically adjusted according to the task requirements, environmental characteristics or system errors. However, if the adjustment rate is too fast, it may cause the system dynamic response to change suddenly, causing control instability or even exciting the system resonance. Therefore, in addition to imposing upper and lower limit constraints on the stiffness and damping itself, the rate of change thereof also needs to be constrained to ensure the smoothness of the adjustment process and the stability of the system.

[0067] Only by replacing into the vector form, the rate of change constraint design in variable impedance control can be obtained:

[0068] And the passivity constraint described above can be constructed in the following way: According to the position error, the differential of the position error, the inertia matrix and the stiffness matrix, the total energy of the system is calculated; the total energy change is obtained by derivation of the total energy of the system; the external force input power is obtained based on the differential of the position error and the external force; the passivity constraint is constructed by combining the total energy change and the external force input power, so that the total energy change is less than the external force input power.

[0069] First, the stability of free motion, the dynamic equation of free motion, that is, the contact without external force, can be obtained:

[0070] In order to analyze the stability of the system, the following Lyapunov function is constructed , W The total energy of the system can be represented as:

[0071] Among them, the first term represents the change of system kinetic energy, and the second term represents the change of potential energy. Note that the inertia matrix and the stiffness matrix are symmetric matrices, so the total energy change can be obtained by derivation of :

[0072] The dynamic equation is brought into the total energy change equation to obtain:

[0073] The above formula shows that the change of system energy is subject to the rate of change of inertia matrix, damping dissipation term and stiffness change term.

[0074] To ensure the stability of the system without external force disturbance, the following conditions are set:

[0075] That is, the rate of change of the damping and stiffness matrix needs to be designed to offset the instability factors caused by the change of inertia.

[0076] When the system is subjected to external force , the calculation update of the total energy change is as follows:

[0077] To ensure the stability of the system without external force disturbance, the following conditions are set:

[0078] That is:

[0079] The above formula shows that the total energy change of the system is less than the input power of the external force, so the system cannot generate energy by itself and can only consume or store the energy input by the outside world, meeting the definition of passivity.

[0080] Combining the above multiple constraint conditions, the total inequality constraint of the variable impedance control can be constructed as follows:

[0081] Under the consideration of the above multiple constraint conditions, the non-target function under multiple constraint conditions is constructed as follows:

[0082] Let the state vector and input vector obtained by the th iteration be , , , } and , , , , respectively. The two vectors are spliced into an iteration vector, denoted as .

[0083] On this basis, the optimization model is used to perform iteration of the iteration vector based on the constructed nonlinear target function and iteration vector under the constraint of the set multiple constraint conditions until the preset requirements are met. This can be achieved in the following way: The optimization model is used to perform quadratic programming on the iteration vector to obtain a variable increment corresponding to the iteration vector; a constraint and an optimization sub-function of the variable increment are constructed; and the optimization sub-function is iterated under the constraint of the variable increment until an absolute value of the variable increment is less than a preset threshold, and it is determined that a preset requirement is met.

[0084] In this embodiment, the sequential quadratic programming method is used to solve the case where the nonlinear objective function and the complex constraint condition are included in the optimization model. The method approximates the optimal solution of the original nonlinear programming problem by solving an approximate second-order programming sub-problem in each iteration. Specifically, the steps of constructing the constraint and the optimization sub-function of the variable increment can be implemented in the following way: The first-order derivative gradient and the second-order derivative Hessian matrix of the constructed nonlinear objective function with respect to the iteration vector are calculated; the state vector and the control vector in the iteration vector are differentiated respectively, and the constraint function constructed by the set of multiple constraint conditions is differentiated to obtain iteration coefficients; and the constraint and the optimization sub-function of the variable increment are obtained based on the iteration coefficients, the first-order derivative gradient and the second-order derivative Hessian matrix.

[0085] In this embodiment, at the current point , the first-order derivative gradient and the second-order derivative Hessian matrix of the nonlinear objective function are calculated:

[0086] The iteration coefficients are obtained by differentiating the dynamic matrix with respect to the state and action: , , which satisfy:

[0087] Similarly, the iteration coefficients are obtained by differentiating the constraint function : , , which satisfy:

[0088] The above calculation formula is converted into the form with respect to :

[0089] The above constraint condition is used to construct the optimization sub-function of the variable increment at the current iteration point as follows:

[0090] By solving the optimization problem under the above optimization sub-function, the optimized variable increment of the current iteration step can be obtained . Further, the step is adjusted by using the line search strategy to realize the update iteration of the iteration vector as the variable:

[0091] When the absolute value of the variable increment is less than the preset threshold, the control vector in the iteration vector at this time is extracted as the current input.

[0092] On the basis of the above, the impedance parameter vector and the stiffness parameter vector are obtained based on the extracted control vector. Specifically, the elements in the extracted control vector are integrated to obtain the impedance parameter vector and the stiffness parameter vector.

[0093] The impedance parameter vector and the stiffness parameter vector are diagonal elements of the impedance matrix and the stiffness matrix respectively, and the impedance parameter vector and the stiffness parameter vector are respectively expanded by adding zero elements to obtain the impedance matrix and the stiffness matrix .

[0094] The obtained impedance matrix and stiffness matrix are input into the impedance controller, and the torque for executing robot control is predicted by the impedance controller, so as to control the pose of the robot at the next time.

[0095] Please refer to Figure 2 , which is a schematic diagram of a polishing simulation scene built in the simulation software matlab by using the tool robotic tools. In the related simulation scene, under the control scheme provided by the embodiment, the normal contact force of the robot changes over time as shown in Figure 3 , and Figure 3 It can be seen that the robot maintains stable contact in the normal direction.

[0096] In addition, low impedance, high impedance and variable impedance tests are also performed, and the displacement curve graphs of the x direction of the three groups of tests are respectively shown in Figure 4 , Figure 5 and Figure 6 . In the curve graphs of the three groups of tests, the solid line represents the actual trajectory curve, and the dashed line represents the desired trajectory curve. As can be seen from the graphs, in the low stiffness and damping control test, it is difficult to track the desired trajectory, and there is a larger tracking error. Under high stiffness, it can maintain high tracking, and through impedance change, similar effect to high stiffness can be achieved, with good tracking effect.

[0097] Based on the same inventive concept, please refer to Figure 7The embodiment of the present application also provides a functional module schematic diagram of the robot variable impedance control system based on model predictive control, and the robot variable impedance control system based on model predictive control can be divided into functional modules according to the method embodiment. For example, each functional module can be divided according to each function, or two or more functions can be integrated in one processing module. The integrated module can be realized in the form of hardware or in the form of a software functional module. It should be noted that the division of the module in the embodiment of the present application is illustrative, and is only a logical function division, and another division mode can be used in actual implementation.

[0098] For example, in the case of dividing each functional module according to each function, Figure 7 The robot variable impedance control system based on model predictive control shown is only a device schematic diagram. The robot variable impedance control system based on model predictive control can include an obtaining module, an iteration module, an extracting module, an expanding module and a predictive control module, and the functions of each functional module of the robot variable impedance control system based on model predictive control are described in detail below.

[0099] The obtaining module is used to obtain a current state vector and a control vector in a robot control process, and an iteration vector is constructed based on the state vector and the control vector; The iteration module is used to perform iteration of the iteration vector under the constraint of a plurality of constraint conditions by using an optimization model based on the constructed nonlinear objective function and the iteration vector, until a preset requirement is met, and the plurality of constraint conditions include upper and lower bound constraints of impedance parameters and stiffness parameters, a variation rate constraint, and a passivity constraint; The extracting module is used to extract the control vector in the iteration vector, and an impedance parameter vector and a stiffness parameter vector are obtained based on the extracted control vector; The expanding module is used to expand the impedance parameter vector and the stiffness parameter vector to obtain an impedance matrix and a stiffness matrix; The predictive control module is used to input the impedance matrix and the stiffness matrix into an impedance controller, and a torque for executing robot control is predicted.

[0100] The robot variable impedance control system based on model predictive control provided in the embodiment can be used to execute the robot variable impedance control method based on model predictive control in any of the implementation manners in the above embodiments, and details not described in the embodiment can be referred to the corresponding description in the above embodiments, which will not be described herein again.

[0101] Please refer to Figure 8A structural block diagram of a control device provided by the embodiment of the present application is shown in the figure, which can be a computer device, a server or the like in a control platform. The control device comprises a memory, a processor and a communication module. The memory, the processor and the communication module are electrically connected with each other directly or indirectly to realize data transmission or interaction. For example, the elements can be electrically connected with each other through one or more communication buses or signal lines.

[0102] The memory is used for storing computer programs or data. The memory can be, but is not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read only memory (PROM), an erasable programmable read only memory (EPROM), an electric erasable programmable read only memory (EEPROM) and the like.

[0103] The processor is used for reading / writing the data or programs stored in the memory and executing the robot variable impedance control method based on model predictive control provided by any embodiment of the present application.

[0104] The communication module is used for establishing a communication connection between the control device and other communication terminals through a network and for receiving / transmitting data through the network.

[0105] It should be understood that, Figure 8 The structure shown in the figure is only a structural schematic diagram of the control device, and the control device can further comprise more or less components than those shown in the figure or have a different configuration from that shown in the figure. Figure 8 Figure 8

[0106] Further, the embodiment of the present application further provides a computer readable storage medium, which stores machine executable instructions. When the machine executable instructions are executed, the robot variable impedance control method based on model predictive control provided by the above embodiment is realized.

[0107] Specifically, the computer readable storage medium can be a general storage medium such as a mobile disk, a hard disk or the like. When the computer program on the computer readable storage medium is run, the robot variable impedance control method based on model predictive control can be executed. For the process involved when the computer readable storage medium and the executable instructions thereof are run, reference can be made to the related description in the above method embodiment, which will not be described in detail here. ​​​​​​

[0108] In the embodiments of the present application, it should be understood that the disclosed device and method can be implemented in other ways. The device embodiments described above are only illustrative. For example, the division of the units is only a logical function division. There can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some communication interfaces, devices or units, and can be electrical, mechanical or other forms.

[0109] In addition, the units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, that is, they can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiments.

[0110] In addition, the functional modules in each embodiment of the present application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0111] It should be noted that if the functions are realized in the form of software function modules and sold or used as independent products, they can be stored in a computer readable storage medium. Based on this understanding, the technical solutions of the present application essentially or the parts that make contributions to the prior art or parts of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a number of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various media that can store program codes.

[0112] The above only describes the embodiments of the present application and does not limit the protection scope of the present application. For those skilled in the art, the present application can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A robot variable impedance control method based on model predictive control, characterized in that: The method comprises: Acquire a current state vector and a control vector during the robot control process, and construct an iteration vector based on the state vector and the control vector; Using the optimization model based on the constructed nonlinear objective function and the iteration vector, the iteration vector is iterated under the constraints of multiple set constraints until preset requirements are met, the multiple constraints including upper and lower bound constraints of impedance parameters, stiffness parameters, rate of change constraints, and passivity constraints; extracting a control vector from the iterative vector, and obtaining an impedance parameter vector and a stiffness parameter vector based on the extracted control vector; Expanding the impedance parameter vector and the stiffness parameter vector to obtain an impedance matrix and a stiffness matrix; The impedance matrix and the stiffness matrix are input into the impedance controller to predict the torque for executing robot control.

2. The robot variable impedance control method based on model predictive control according to claim 1, characterized in that: The step of obtaining the current state vector and control vector during the robot control process includes: Obtain the current position error, impedance parameters, stiffness parameters and external forces during robot control; constructing a state vector based on the position error, impedance parameter, stiffness parameter and external force; A control vector is constructed based on the impedance parameter and the stiffness parameter.

3. The robot variable impedance control method based on model predictive control according to claim 1, characterized in that: The nonlinear objective function is constructed in the following way: Based on the Cartesian pose of the robot end at the initial planning moment, the Cartesian inertia matrix is ​​obtained; Constructing a continuous-time state equation based on the control vector and the Cartesian inertia matrix; discretizing the continuous-time state equation to obtain a discrete-time state equation; Under the discrete-time state equation and multiple set constraints, a nonlinear objective function constructed by a stage cost function and a terminal cost function is constructed based on the state vector and the control vector.

4. The robot variable impedance control method based on model predictive control according to claim 3, characterized in that: The stage cost function is constructed in the following way: constructing a reference stiffness function that varies with position error, and obtaining a stiffness adjustment rate based on the reference stiffness function; constructing an impedance coefficient based on the stiffness adjustment rate; Obtaining a state reference item according to the stiffness adjustment rate, the impedance coefficient, and the external force; A stage cost function about the control vector and the state vector is constructed based on the state reference item, the control vector and the state vector.

5. The robot variable impedance control method based on model predictive control according to claim 1, characterized in that: The passivity constraint in the plurality of constraints is constructed in the following way: The total energy of the system is calculated based on the position error, the differential of the position error, the inertia matrix and the stiffness matrix; Derivative the total energy of the system to obtain a total energy change; Obtaining an external force input power based on a differential of the position error and the external force; A passivity constraint is constructed by combining the total energy change and the external force input power, so that the total energy change is smaller than the external force input power.

6. The robot variable impedance control method based on model predictive control according to claim 1, characterized in that: The step of utilizing the optimization model based on the constructed nonlinear objective function and the iteration vector, and executing iteration of the iteration vector under the constraints of a plurality of set constraints until preset requirements are met, includes: Performing quadratic programming on the iteration vector using an optimization model to obtain a variable increment corresponding to the iteration vector; Constructing constraints and optimization subfunctions for the variable increments; The optimization sub-function is iterated under the constraint of the variable increment until the absolute value of the variable increment is less than a preset threshold, and it is determined that the preset requirement is met.

7. The robot variable impedance control method based on model predictive control according to claim 6, characterized in that: The step of constructing the constraint and optimization sub-function of the variable increment includes: Calculating the first-order derivative gradient and the second-order derivative Hazen matrix of the constructed nonlinear objective function with respect to the iteration vector; Differentiating the state vector and the control vector in the iteration vector respectively, and differentiating the constraint function constructed by the set multiple constraint conditions to obtain an iteration coefficient; Constraints on variable increments and optimization subfunctions are obtained based on the iteration coefficient, the first-order derivative gradient and the second-order derivative Hazen matrix.

8. The robot variable impedance control method based on model predictive control according to claim 1, characterized in that: The step of obtaining the impedance parameter vector and the stiffness parameter vector based on the extracted control vector includes: The elements in the extracted control vector are integrated respectively to obtain the impedance parameter vector and the stiffness parameter vector.

9. A robot variable impedance control system based on model predictive control, characterized in that: The system comprises: An acquisition module is used to obtain the current state vector and control vector during the robot control process, and construct an iteration vector based on the state vector and control vector; an iterative module, configured to utilize the nonlinear objective function constructed based on the optimization model and the iterative vector to iterate the iterative vector under the constraints of a plurality of set constraints until preset requirements are met, wherein the plurality of constraints include upper and lower bound constraints of an impedance parameter and a stiffness parameter, a rate of change constraint, and a passivity constraint; an extraction module, configured to extract a control vector from the iterative vector, and obtain an impedance parameter vector and a stiffness parameter vector based on the extracted control vector; An expansion module, configured to expand the impedance parameter vector and the stiffness parameter vector to obtain an impedance matrix and a stiffness matrix; The predictive control module is used to input the impedance matrix and the stiffness matrix into the impedance controller to predict the torque for executing robot control.

10. A control device, characterized in that: The device comprises one or more storage media and one or more processors communicating with the storage media, wherein the one or more storage media store machine-executable instructions executable by the processors, and when the control device is running, the processors execute the machine-executable instructions to perform the method described in any one of claims 1 to 8.

Images