Self-adaptive control method for topological structure of homogeneous modular robot

Abstract

The embodiment of the invention provides an adaptive control method for a topological structure of a homogeneous modular robot, which realizes optimal control of the modular robot under external interference and comprises the following steps of: obtaining mathematical representation of the topological structure of the homogeneous modular robot, a three-dimensional model of a modular unit and a three-dimensional model of the modular unit; then a kinetic equation, a state updating equation, a cost function, a value function and an action value function of the homogeneous modular robot are obtained, then first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the state updating equation, the value function and the action value function of the homogeneous modular robot to contained variables are calculated, and then an optimal controller and a nonlinear disturbance observer of the homogeneous modular robot are obtained; and designing a homogeneous modular robot distributed optimal control method integrated with the nonlinear disturbance observer, and obtaining a homogeneous modular robot topological structure adaptive controller integrated with the nonlinear disturbance observer. According to the technical scheme provided by the embodiment of the invention, the optimal control of the modular robot under any topological structure can be realized.

Description

[Technical field]

[0001] The invention relates to a self-adaptive control method for a homogeneous modular robot topology structure, and belongs to the field of robot control. [Background technology]

[0002] With the further promotion of industrial automation, the application of robots in various fields has gradually become popular. Robots with single performance and fixed structure will be difficult to apply to tasks with diverse loads and requirements. The concept of modular robots was proposed in 1988. Such robots are generally composed of multiple identical modules (homogeneous) or different modules (heterogeneous). By changing the number of modules and their relative positions, the robot can be reconstructed into a new configuration and obtain new functions to adapt to the changing working environment and meet the diverse task requirements. Therefore, research on modular robots can promote the further development of my country's robotics field.

[0003] Compared with heterogeneous modular robots, homogeneous modular robot modules can replace each other, with strong module scalability and versatility, and can be used in batches for design, manufacturing, assembly, maintenance and other related work, with high ease of use. Therefore, homogeneous modular robots have a wider range of application scenarios. In the actual application process of robots, they will be affected by external interference, limited energy and other factors. At the same time, the topological structure of modular robots is not fixed during application. Therefore, it is of great theoretical research value to carry out optimal control research that can adapt to homogeneous modular robots. [Summary of the invention]

[0004] In view of this, the present invention provides a topology adaptive control method for a homogeneous modular robot to achieve optimal control of the homogeneous modular robot.

[0005] An embodiment of the present invention provides a method for adaptively controlling a homogeneous modular robot topology structure, comprising:

[0006] Obtain the mathematical representation of the topological structure of the homogeneous modular robot and the three-dimensional model of the modular unit;

[0007] Based on the mathematical representation of the topological structure of the homogeneous modular robot and the three-dimensional model of the module unit, the dynamic equation of the homogeneous modular robot is obtained;

[0008] According to the dynamic equation of the homogeneous modular robot, the state update equation of the homogeneous modular robot is obtained;

[0009] According to the state update equation of the homogeneous modular robot, the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the state update equation of the homogeneous modular robot with respect to the variables contained are obtained;

[0010] According to the state update equation of the homogeneous modular robot, the cost function of the homogeneous modular robot is obtained;

[0011] According to the cost function of the homogeneous modular robot, the value function of the homogeneous modular robot is obtained;

[0012] According to the homogeneous modular robot cost function and the homogeneous modular robot value function, the homogeneous modular robot action value function is obtained;

[0013] According to the homogeneous modular robot value function, the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the homogeneous modular robot value function with respect to the contained variables are obtained;

[0014] According to the action value function of the homogeneous modular robot, the first-order partial derivative, the second-order partial derivative and the second-order mixed partial derivative of the action value function of the homogeneous modular robot with respect to the contained variables are obtained;

[0015] According to the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the state update equation of the homogeneous modular robot with respect to the variables contained, the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the value function of the homogeneous modular robot with respect to the variables contained, and the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the action value function of the homogeneous modular robot with respect to the variables contained, the optimal controller of the homogeneous modular robot is obtained;

[0016] According to the dynamic equation of homogeneous modular robot, the nonlinear disturbance observer of homogeneous modular robot is obtained.

[0017] Based on the mathematical representation of the homogeneous modular robot topology structure, the three-dimensional model of the modular unit, the optimal controller of the homogeneous modular robot and the nonlinear disturbance observer of the homogeneous modular robot, a distributed optimal control method of the homogeneous modular robot with nonlinear disturbance observer is designed, and an adaptive controller of the homogeneous modular robot topology structure with nonlinear disturbance observer is obtained.

[0018] In the above method, the dynamic equation of the homogeneous modular robot is obtained based on the mathematical representation of the topological structure of the homogeneous modular robot and the three-dimensional model of the module unit, including:

[0019] For n topo_module The topological structure of a homogeneous modular robot with modules is mathematically represented as

[0020]

[0021] Among them, n topo_module Represents the number of modules contained in the homogeneous modular robot topology; m i (i=1,2,...,n topo_module ) represents the module numbers in the homogeneous modular robot topology; the matrix B is called the grounding relationship matrix, ba i (i=1,2,...,ntopo_module ) indicates whether the module in the homogeneous modular robot topology is grounded as a base when used, ba i =0 means module m i When not grounded, i ≠0 means module m i When using ba i The interface ground is used as the base; the matrix C is called the connection relationship matrix of the homogeneous modular robot, c ij (i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module )∈{1,...,n surf_con} indicates module m i With module m j The number of the connected interface. When i=j, c ij =0(i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module ), n surf_con represents the number of surface interfaces of a single module; the matrix O is called the connection orientation relationship matrix of the homogeneous modular robot, ij (i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module ) indicates module m i With module m j The connection orientation relationship, when i = j, co ij =0(i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module );

[0022] The dynamic model of a homogeneous modular robot with m modular units and n degrees of freedom is:

[0023]

[0024] in, represents the inertia force matrix; | represents the Coriolis force and centrifugal force terms on each joint during the motion of the homogeneous modular robot; represents the gravitational force on the homogeneous modular robot during its motion; They represent joint angle, joint angular velocity, joint angular acceleration, and driving torque respectively.

[0025] In the above method, the state update equation of the homogeneous modular robot is obtained according to the dynamic equation of the homogeneous modular robot, including:

[0026] According to the dynamic equation of homogeneous modular robot, we can get:

[0027]

[0028] in, represents the inertia force matrix; It represents the Coriolis force and centrifugal force terms on each joint during the motion of the homogeneous modular robot; represents the gravitational force on the homogeneous modular robot during its motion; They represent joint angle, joint angular velocity, joint angular acceleration, and driving torque respectively, and n represents the number of degrees of freedom in the homogeneous modular robot;

[0029] make is the system state quantity, As the system input, we can get:

[0030]

[0031] After finishing, we can get:

[0032]

[0033] in, ψ(x) is given by M -1 (x), C(x), G(x);

[0034] The state update equation of the homogeneous modular robot is obtained as:

[0035]

[0036] Where x| t 、x| t+1 Respectively represent the system state at time t and time t+1, u| t represents the system input at time t, and ε represents the time interval, which is set according to task requirements.

[0037] In the above method, the cost function of the homogeneous modular robot is obtained according to the state update equation of the homogeneous modular robot, including:

[0038] The cost function of the design system in a certain state is:

[0039]

[0040] Where x| t 、u| t represents the system state quantity and system input quantity at time t, x| t,d represents the expected system state at time t, e = x| t -x| t,d represents the system state error at time t, ug | t,d represents the joint driving torque corresponding to the compensation of gravity in the desired system state at time t, The weight matrix corresponding to the error term and the torque term of the system state quantity is a positive definite matrix, which is set according to the task requirements. n represents the number of degrees of freedom in the homogeneous modular robot;

[0041] Starting from the system initial state x0, in the system input sequence U={u| c ,u|1,...,u| N-1}, the total cost of the entire movement process is:

[0042]

[0043] Where N represents the number of movement steps.

[0044] In the above method, the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the state update equation of the homogeneous modular robot with respect to the variables contained therein, the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the value function of the homogeneous modular robot with respect to the variables contained therein, and the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the action value function of the homogeneous modular robot with respect to the variables contained therein are obtained, comprising:

[0045] The optimal controller for a homogeneous modular robot is:

[0046]

[0047] in, Represents the second-order partial derivative of the action value function with respect to the system input in u| i Take the value at Represents the second-order mixed partial derivative of the action value function to the system input and system state quantity in x| i 、u| i Take the value at Represents the first-order partial derivative of the action value function with respect to the system input in u| i Take the value at, Δx| i It represents the difference between the system state quantities of two consecutive calculations at time i in the optimal control calculation process, Δu| i It represents the difference between the system inputs of two consecutive calculations at time i in the optimal control calculation process.

[0048] It can be seen from the above technical solutions that the embodiments of the present invention have the following beneficial effects:

[0049] In the technical solution of the embodiment of the present invention, a mathematical representation of the topological structure of a homogeneous modular robot and a three-dimensional model of a module unit are obtained, and then the dynamic equation, state update equation, cost function, value function, and action value function of the homogeneous modular robot are obtained, and then the first-order partial derivatives, second-order partial derivatives, and second-order mixed partial derivatives of the state update equation, value function, and action value function of the homogeneous modular robot with respect to the variables contained are calculated, and then its optimal controller and nonlinear disturbance observer are obtained, and a distributed optimal control method of a homogeneous modular robot incorporating a nonlinear disturbance observer is designed, and a distributed optimal controller of a homogeneous modular robot incorporating a nonlinear disturbance observer is obtained. According to the technical solution provided by the embodiment of the present invention, the optimal control of a homogeneous modular robot can be achieved. Combined with the characteristics of the homogeneous modular robot, based on the three-dimensional model of the module unit, the optimal control of the modular robot under any topological structure is achieved, providing a control basis for the application of the modular robot.

Brief Description of the Drawings

[0050] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It is obvious that the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creativity and labor.

[0051] Figure 1 It is a schematic diagram of a flow chart of a method for adaptively controlling a homogeneous modular robot topology structure provided by an embodiment of the present invention;

[0052] Figure 2 is a schematic diagram of a three-dimensional model of a module unit in an embodiment of the present invention;

[0053] Figure 3 is the convergence curve of the objective function of the homogeneous modular robot in the embodiment of the present invention;

[0054] Figure 4 is an optimization rate curve of the objective function of the homogeneous modular robot in an embodiment of the present invention;

[0055] Figure 5 This is the trajectory tracking situation of the end of the homogeneous modular robot in the embodiment of the present invention. [Specific embodiment]

[0056] In order to better understand the technical solution of the present invention, the embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0057] It should be clear that the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0058] The embodiment of the present invention provides a method for adaptive control of a homogeneous modular robot topology structure. Figure 1 , which is a flow chart of a method for adaptive control of a homogeneous modular robot topology structure incorporating a nonlinear disturbance observer provided by an embodiment of the present invention, such as Figure 1 As shown, the method comprises the following steps:

[0059] Step 101, obtaining a mathematical representation of a homogeneous modular robot topology structure and a three-dimensional model of a module unit.

[0060] Specifically, the module unit three-dimensional model is as follows Figure 2 As shown, 1 represents the connecting plane of two hemispherical shells; 2 and 18 represent two hemispherical shells; 3, 6, 11 and 15 represent the positioning device on the male interface; 4 and 14 represent the torque output shaft of the motor module of the active interface; 5 and 12 represent the motor module body on the active interface; 7, 10, 13 and 20 represent the mechanical structure components on the interface; 8, 9, 19 and 21 represent the positioning grooves on the female interface that match the positioning device; 16 represents the motor module body at the center of the module unit; 17 represents the torque output shaft of the motor module at the center of the module unit;

[0061] The topological structure of the homogeneous modular robot is mathematically represented as

[0062] M = [1,3,2,4]

[0063] BA=[2,0,0,0]

[0064]

[0065] Step 102, obtaining a dynamic equation of the homogeneous modular robot based on a mathematical representation of the topological structure of the homogeneous modular robot and a three-dimensional model of a module unit.

[0066] Specifically, the dynamic model of a homogeneous modular robot with m modular units and n degrees of freedom is:

[0067]

[0068] in, represents the inertia force matrix; It represents the Coriolis force and centrifugal force terms on each joint during the motion of the homogeneous modular robot; represents the gravitational force on the homogeneous modular robot during its motion; They represent joint angle, joint angular velocity, joint angular acceleration, and driving torque respectively.

[0069] Step 103, obtaining a state update equation of the homogeneous modular robot according to the dynamic equation of the homogeneous modular robot.

[0070] Specifically, according to the dynamic equation of homogeneous modular robot, we can get:

[0071]

[0072] in, represents the inertia force matrix; It represents the Coriolis force and centrifugal force terms on each joint during the motion of the homogeneous modular robot; represents the gravitational force on the homogeneous modular robot during its motion; They represent joint angle, joint angular velocity, joint angular acceleration, and driving torque respectively, and n represents the number of degrees of freedom in the homogeneous modular robot;

[0073] make is the system state quantity, As the system input, we can get:

[0074]

[0075] After finishing, we can get:

[0076]

[0077] in, ψ(x) is given by M -1 (x), C(x), G(x);

[0078] The state update equation of the homogeneous modular robot is obtained as:

[0079]

[0080] Where x| t 、x| t+1 Respectively represent the system state at time t and time t+1, u| t represents the system input at time t, and ε represents the time interval, which is set to 10 according to the task requirements. -3 Second.

[0081] Step 104, based on the homogeneous modular robot state update equation, obtain the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the homogeneous modular robot state update equation with respect to the variables contained therein.

[0082] Specifically, using Taylor's formula, the state update equation is i ,u| i ) and linearize it to get

[0083]

[0084] The first-order partial derivative of function Θ with respect to x is

[0085]

[0086] The first-order partial derivative of function Θ with respect to u is

[0087]

[0088] The state update equation is in (x| i ,u| i ) are linearized in the vicinity of

[0089]

[0090] Among them, I 2n is the identity matrix of dimension 2n×2n, The dimension is 4n 2 ×4n 2 The switching matrix, represents the Kronecker product, They are the second-order partial derivative and second-order mixed partial derivative of function Θ with respect to x and u, respectively, and are recorded as ▽ in gradient form. xx Θ、▽ uu Θ、▽ xu Θ、▽ ux Θ, and ▽ xu T Θ=▽ ux Θ.

[0091] Step 105, obtaining a homogeneous modular robot cost function according to a homogeneous modular robot state update equation.

[0092] Specifically, the cost function of a certain state of the design system is:

[0093]

[0094] Where x| t 、u| t represents the system state quantity and system input quantity at time t, x| t,d represents the expected system state at time t, e = x| t -x| t,d represents the system state error at time t, u g | t,d represents the joint driving torque corresponding to the compensation of gravity in the desired system state at time t, The weight matrix corresponding to the error term and the moment term of the system state quantity is a positive definite matrix and is set according to the task requirements. The diagonal elements are 10, and the rest of the elements are 0. The diagonal elements are 1, and the rest of the elements are 0;

[0095] Starting from the system initial state x|0, in the system input sequence U={u|0,u|1,…,u| N-1}, the total cost of the entire movement process is:

[0096]

[0097] Where N represents the number of movement steps.

[0098] Step 106, obtaining a homogeneous modular robot value function according to the homogeneous modular robot cost function.

[0099] Step 107, obtaining the homogeneous modular robot action value function according to the homogeneous modular robot cost function and the homogeneous modular robot value function.

[0100] Step 108, based on the homogeneous modular robot value function, obtain the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the homogeneous modular robot value function with respect to the variables contained therein.

[0101] Step 109, based on the action value function of the homogeneous modular robot, obtain the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the action value function of the homogeneous modular robot with respect to the variables contained therein.

[0102] Step 110, obtaining the optimal controller of the homogeneous modular robot based on the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the state update equation of the homogeneous modular robot with respect to the variables contained therein, the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the value function of the homogeneous modular robot with respect to the variables contained therein, and the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the action value function of the homogeneous modular robot with respect to the variables contained therein.

[0103] Specifically, the optimal controller for a homogeneous modular robot is:

[0104]

[0105] in, Represents the second-order partial derivative of the action value function with respect to the system input in u| i Take the value at Represents the second-order mixed partial derivative of the action value function to the system input and system state quantity in x| i 、u| i Take the value at Represents the first-order partial derivative of the action value function with respect to the system input in u| i Take the value at, Δx| iIt represents the difference between the system state quantities of two consecutive calculations at time i in the optimal control calculation process, Δu| i It represents the difference between the system inputs of two consecutive calculations at time i in the optimal control calculation process.

[0106] Step 111: obtaining a nonlinear disturbance observer of the homogeneous modular robot according to the dynamic equation of the homogeneous modular robot.

[0107] Specifically, the nonlinear disturbance observer of module unit i is

[0108]

[0109] in, represents the disturbance observer gain matrix, Represents the module unit interference term τ di Estimated value, M represents the module unit dynamics equation i (θ i ), G i (θ i )’s nominal parameters, θ i , They represent the joint angle, joint angular velocity, and joint angular acceleration in the module unit respectively.

[0110] Introducing auxiliary state variables in

[0111] The nonlinear state observer of the homogeneous modular robot is

[0112]

[0113] Step 112, based on the mathematical representation of the homogeneous modular robot topology structure, the three-dimensional model of the module unit, the optimal controller of the homogeneous modular robot, and the nonlinear disturbance observer of the homogeneous modular robot, a distributed optimal control method of the homogeneous modular robot incorporating the nonlinear disturbance observer is designed to obtain an adaptive controller of the homogeneous modular robot topology structure incorporating the nonlinear disturbance observer.

[0114] Specifically, the adaptive controller of homogeneous modular robot topology structure incorporating nonlinear disturbance observer is:

[0115]

[0116] The initial state of each module unit is thetalist ini =[10°,10°,10°] T , and its corresponding terminal expected state is EEF end=[-0.4658m,0.0414m,-0.2289m,-159.1376°,-32.3454°,-108.1587°] T According to the above method provided by the embodiment of the present invention, simulation was performed.

[0117] The trajectory tracking is performed by using the adaptive control of the homogeneous modular robot topology structure with nonlinear disturbance observer. During the optimal control optimization process, the convergence curve of the objective function of the homogeneous modular robot is as follows: Figure 3 As shown, the optimization rate curve of the objective function of the homogeneous modular robot is as follows Figure 4 As shown. Figure 3 and Figure 4 It can be seen that as the iteration process proceeds, the whole machine cost calculated by the objective function value of the homogeneous modular robot shows a convergence trend, and the objective function optimization rate also shows a convergence trend. When iterating to the 86th generation, the target change of the homogeneous modular robot is 8.2979×10 -6 , the objective function value of the homogeneous modular robot is 0.0854. Using the control sequence after iterative convergence as the system input, the end trajectory tracking of the homogeneous modular robot is obtained as follows Figure 5 As shown, Figure 5 (a) is the comparison diagram of the terminal three-dimensional trajectory. Figure 5 (b) is a comparison chart of the actual and expected end positions. Figure 5 (c) is a comparison of the position errors at the beginning of the iteration and at the end of the convergence. Figure 5 (d) is the motion trajectory of each joint, Figure 5 (e) is the driving torque of each joint. By comparison, it can be concluded that after iterative convergence, the tracking error of the modular robot end is reduced from 10 -1 Order of magnitude reduced by 10 -3 The tracking accuracy is significantly improved, which proves the correctness and effectiveness of the adaptive control method of homogeneous modular robot topology structure integrating nonlinear disturbance observer.

[0118] The technical solution of the embodiment of the present invention has the following beneficial effects:

[0119] In the technical solution of the embodiment of the present invention, a mathematical representation of the topological structure of a homogeneous modular robot and a three-dimensional model of a module unit are obtained, and then the dynamic equation, state update equation, cost function, value function, and action value function of the homogeneous modular robot are obtained, and then the first-order partial derivatives, second-order partial derivatives, and second-order mixed partial derivatives of the state update equation, value function, and action value function of the homogeneous modular robot with respect to the variables contained are calculated, and then its optimal controller and nonlinear disturbance observer are obtained, and a distributed optimal control method of a homogeneous modular robot incorporating a nonlinear disturbance observer is designed, and a distributed optimal controller of a homogeneous modular robot incorporating a nonlinear disturbance observer is obtained. According to the technical solution provided by the embodiment of the present invention, the optimal control of a homogeneous modular robot can be achieved. Combined with the characteristics of the homogeneous modular robot, based on the three-dimensional model of the module unit, the optimal control of the modular robot under any topological structure is achieved, providing a control basis for the application of the modular robot.

[0120] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

[0121] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A method for adaptive control of homogeneous modular robot topology structure, characterized in that: The method comprises: Obtain the mathematical representation of the topological structure of the homogeneous modular robot and the three-dimensional model of the modular unit; Based on the mathematical representation of the topological structure of the homogeneous modular robot and the three-dimensional model of the module unit, the dynamic equation of the homogeneous modular robot is obtained; According to the dynamic equation of the homogeneous modular robot, the state update equation of the homogeneous modular robot is obtained; According to the state update equation of the homogeneous modular robot, the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the state update equation of the homogeneous modular robot with respect to the variables contained are obtained; According to the state update equation of the homogeneous modular robot, the cost function of the homogeneous modular robot is obtained; According to the cost function of the homogeneous modular robot, the value function of the homogeneous modular robot is obtained; According to the homogeneous modular robot cost function and the homogeneous modular robot value function, the homogeneous modular robot action value function is obtained; According to the homogeneous modular robot value function, the first-order partial derivative, second-order partial derivative and second-order mixed partial derivative of the homogeneous modular robot value function with respect to the contained variables are obtained; According to the action value function of the homogeneous modular robot, the first-order partial derivative, the second-order partial derivative and the second-order mixed partial derivative of the action value function of the homogeneous modular robot with respect to the contained variables are obtained; According to the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the state update equation of the homogeneous modular robot with respect to the variables contained, the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the value function of the homogeneous modular robot with respect to the variables contained, and the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the action value function of the homogeneous modular robot with respect to the variables contained, the optimal controller of the homogeneous modular robot is obtained; According to the dynamic equation of homogeneous modular robot, the nonlinear disturbance observer of homogeneous modular robot is obtained. Based on the mathematical representation of the homogeneous modular robot topology structure, the three-dimensional model of the modular unit, the optimal controller of the homogeneous modular robot and the nonlinear disturbance observer of the homogeneous modular robot, a distributed optimal control method of the homogeneous modular robot with nonlinear disturbance observer is designed, and an adaptive controller of the homogeneous modular robot topology structure with nonlinear disturbance observer is obtained.

2. The method according to claim 1, characterized in that Based on the mathematical representation of the topological structure of the homogeneous modular robot and the three-dimensional model of the module unit, the dynamic equations of the homogeneous modular robot are obtained, including: For n topo_module The topological structure of a homogeneous modular robot with modules is mathematically represented as Among them, n topo_module Represents the number of modules contained in the homogeneous modular robot topology; m i (i=1,2,...,n topo_module ) represents the module numbers in the homogeneous modular robot topology; the matrix B is called the grounding relationship matrix, ba i (i=1,2,...,n topo_module ) indicates whether the module in the homogeneous modular robot topology is grounded as a base when used, ba i =0 means module m i When not grounded, i ≠0 means module m i When using i The interface ground is used as the base; the matrix C is called the connection relationship matrix of the homogeneous modular robot, c ij (i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module )∈{1,...,n surf_con } indicates module m i With module m j The number of the connected interface. When i=j, c ij =0(i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module ), n surf_con represents the number of surface interfaces of a single module; the matrix O is called the connection orientation relationship matrix of the homogeneous modular robot, ij (i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module ) indicates module m i With module m j The connection orientation relationship, when i = j, co ij =0(i=1,2,...,n topo_module ; j = 1, 2, ..., n topo_module ); The dynamic model of a homogeneous modular robot with m modular units and n degrees of freedom is: in, represents the inertia force matrix; It represents the Coriolis force and centrifugal force terms on each joint during the motion of the homogeneous modular robot; represents the gravitational force on the homogeneous modular robot during its motion; They represent joint angle, joint angular velocity, joint angular acceleration, and driving torque respectively.

3. The method according to claim 1, characterized in that According to the dynamic equation of the homogeneous modular robot, the state update equation of the homogeneous modular robot is obtained, including: According to the dynamic equation of homogeneous modular robot, we can get: in, represents the inertia force matrix; It represents the Coriolis force and centrifugal force terms on each joint during the motion of the homogeneous modular robot; represents the gravitational force on the homogeneous modular robot during its motion; They represent joint angle, joint angular velocity, joint angular acceleration, and driving torque respectively, and n represents the number of degrees of freedom in the homogeneous modular robot; make is the system state quantity, As the system input, we can get: After finishing, we can get: in, ψ(x) is given by M -1 (x), C(x), G(x); The state update equation of the homogeneous modular robot is obtained as: Where x| t 、x| t+1 Respectively represent the system state at time t and time t+1, u| t represents the system input at time t, and ε represents the time interval, which is set according to task requirements.

4. The method according to claim 1, characterized in that: According to the state update equation of the homogeneous modular robot, the cost function of the homogeneous modular robot is obtained, including: The cost function of the design system in a certain state is: Where x| t 、u| t represents the system state quantity and system input quantity at time t, x| t,d represents the expected system state at time t, e = x| t -x| t,d represents the system state error at time t, u g | t,d represents the joint driving torque corresponding to the compensation of gravity in the desired system state at time t, The weight matrix corresponding to the error term and the torque term of the system state quantity is a positive definite matrix and is set according to the task requirements. n represents the number of degrees of freedom in the homogeneous modular robot; Starting from the system initial state x|0, in the system input sequence U={u|0,u|1,...,u| N-1 }, the total cost of the entire movement process is: Where N represents the number of movement steps.

5. The method according to claim 1, characterized in that According to the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the state update equation of the homogeneous modular robot with respect to the variables contained, the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the value function of the homogeneous modular robot with respect to the variables contained, and the first-order partial derivatives, second-order partial derivatives and second-order mixed partial derivatives of the action value function of the homogeneous modular robot with respect to the variables contained, the optimal controller of the homogeneous modular robot is obtained, including: The optimal controller for a homogeneous modular robot is: in, Represents the second-order partial derivative of the action value function with respect to the system input in u| i Take the value at Represents the second-order mixed partial derivative of the action value function to the system input and system state quantity in x| i 、u| i Take the value at Represents the first-order partial derivative of the action value function with respect to the system input in u| i Take the value at, Δx| i It represents the difference between the system state quantities of two consecutive calculations at time i in the optimal control calculation process, Δu| i It represents the difference between the system inputs of two consecutive calculations at time i in the optimal control calculation process.

Images