Robot balance control method, device, readable storage medium and robot

Abstract

The application belongs to the technical field of robots, and particularly relates to a robot balance control method and device, a computer readable storage medium and a robot. The method comprises the following steps: simplifying a leg part and a trunk of the double-wheel-foot robot, and establishing a double-wheel-foot flywheel inverted pendulum model of the double-wheel-foot robot; performing dynamics analysis on the double-wheel-foot flywheel inverted pendulum model, and determining a dynamics model expression of the double-wheel-foot robot; and performing model predictive control on the double-wheel-foot robot according to the dynamics model expression, so as to maintain the balance of the double-wheel-foot robot. Through the above method, dynamics analysis can be performed on the established double-wheel-foot flywheel inverted pendulum model to obtain a dynamics model expression, and model predictive control can be performed on the double-wheel-foot robot according to the model expression, so that the control amount constraint of the double-wheel-foot robot is realized, and the robustness of the balance control of the double-wheel-foot robot is improved.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a robot balance control method, device, computer-readable storage medium, and robot. Background Art

[0002] In the prior art, balance control of a two-wheeled legged robot is usually achieved by using a control algorithm that combines proportional, integral and differential control (Proportion Integral Differential, PID) or a linear quadratic regulator (Linear Quadratic Regulator, LQR) algorithm.

[0003] However, the balancing controller based on the PID algorithm involves a large number of parameter adjustment processes, and the algorithm has poor robustness; and although the LQR algorithm has improved stability and robustness, the algorithm has large errors and has no constraints on the control quantity, which may cause the control quantity to exceed the limit. Summary of the Invention

[0004] In view of this, the embodiments of the present application provide a robot balance control method, device, computer-readable storage medium and robot to solve the problem that the existing balance control algorithm is difficult to be compatible with robustness and control quantity constraints of a two-wheeled legged robot.

[0005] A first aspect of the embodiments of the present application provides a robot balance control method, which is applied to a two-wheeled legged robot and may include:

[0006] Simplifying the models of the legs and torso of the two-wheeled leg robot and establishing a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot;

[0007] Determine the dynamic equation of the two-wheeled leg robot:

[0008]

[0009]

[0010] Among them, m f is the mass of the rigid body in the two-wheeled flywheel inverted pendulum model, m p is the mass of the connecting rod in the double-wheeled flywheel inverted pendulum model, L fw L is the distance between the rigid body axis and the wheel foot axis, c is the distance between the center of mass of the rigid body and the rotation axis of the wheel foot, is the forward acceleration of the two-wheeled foot robot, θ p is the angle of the connecting rod relative to the vertical plane, is the angular acceleration of the connecting rod relative to the vertical plane, Jp is the moment of inertia of the rigid body around the wheel foot axis, J w is the moment of inertia of the wheel foot around the wheel foot axis, J f is the moment of inertia of the rigid body around the rigid body's axis of rotation, is the angular acceleration of the rigid body relative to the horizontal plane, R is the radius of the wheel foot, D is the width of the two-wheeled foot robot, I p is the moment of inertia of the connecting rod relative to the z-axis, is the yaw acceleration of the connecting rod, τ l is the joint input torque of the left wheel foot, τ r is the joint input torque of the right wheel foot, τ f Input torque to the joints of the rigid body; determine the dynamic model expression of the two-wheeled leg robot according to the dynamic equation;

[0011] Model predictive control is performed on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot.

[0012] In a specific implementation of the first aspect, in the two-wheeled flywheel inverted pendulum model of the two-wheeled robot, the legs of the two-wheeled robot are equivalent to a connecting rod, the torso of the two-wheeled robot is equivalent to a rigid body with mass, the geometric center of the rigid body serves as the center of mass of the rigid body, and the two ends of the connecting rod are respectively connected to the rigid body and the wheel feet of the two-wheeled robot.

[0013] In a specific implementation of the first aspect, performing model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot includes:

[0014] Determine a discretized state transfer matrix expression of the two-wheeled leg robot according to the dynamic model expression;

[0015] Acquire a current state quantity of the two-wheeled leg robot, and determine a predicted state quantity of the two-wheeled leg robot after a predetermined time period according to the current state quantity and the state transfer matrix expression;

[0016] Obtaining an expected state quantity of the two-wheeled leg robot after the predetermined time period, and optimizing and controlling a control quantity of the two-wheeled leg robot according to the predicted state quantity and the expected state quantity to determine a current control quantity of the two-wheeled leg robot;

[0017] The two-wheeled leg robot is controlled according to the current control amount.

[0018] In a specific implementation manner of the first aspect, the optimization control of the control quantity of the double-wheel foot robot according to the predicted state quantity and the expected state quantity comprises:

[0019] calculating a state quantity error between the predicted state quantity and the expected state quantity;

[0020] constructing an optimization objective function according to the state quantity error and the control quantity of the double-wheel foot robot;

[0021] performing quadratic programming solution on the optimization objective function to obtain the control quantity of the double-wheel foot robot at each control time point within the predetermined time length;

[0022] determining the control quantity of the first control time point within the predetermined time length as the current control quantity of the double-wheel foot robot.

[0023] In a specific implementation manner of the first aspect, the optimization objective function is:

[0024]

[0025] s.t.h j (X k )≤0,j∈{1,…,N}

[0026] wherein, U k is a control quantity matrix, X k is a predicted state quantity matrix, is an expected state quantity matrix, Q is a first coefficient matrix, W is a second coefficient matrix, h j (X k ) is a linear constraint equation of N control time points within the predetermined time length.

[0027] The second aspect of the embodiment of the application provides a robot balance control device, which is applied to a double-wheel foot robot, and can comprise:

[0028] a model establishing module, configured to simplify a model of a leg and a trunk of the double-wheel foot robot, and establish a double-wheel foot flywheel inverted pendulum model of the double-wheel foot robot;

[0029] a dynamics model determining module, configured to determine a dynamics equation of the double-wheel foot robot:

[0030]

[0031]

[0032] wherein, m fis the mass of the rigid body in the two-wheeled flywheel inverted pendulum model, m p is the mass of the connecting rod in the double-wheeled flywheel inverted pendulum model, L fw L is the distance between the rigid body axis and the wheel foot axis, c is the distance between the center of mass of the rigid body and the rotation axis of the wheel foot, is the forward acceleration of the two-wheeled foot robot, θ p is the angle of the connecting rod relative to the vertical plane, is the angular acceleration of the connecting rod relative to the vertical plane, J p is the moment of inertia of the rigid body around the wheel foot axis, J w is the moment of inertia of the wheel foot around the wheel foot axis, J f is the moment of inertia of the rigid body around the rigid body's axis of rotation, is the angular acceleration of the rigid body relative to the horizontal plane, R is the radius of the wheel foot, D is the width of the two-wheeled foot robot, I p is the moment of inertia of the connecting rod relative to the z-axis, is the yaw acceleration of the connecting rod, τ l is the joint input torque of the left wheel foot, τ r is the joint input torque of the right wheel foot, τ f Input torque to the joints of the rigid body; determine the dynamic model expression of the two-wheeled leg robot according to the dynamic equation;

[0033] A predictive control module is used to perform model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot.

[0034] In a specific implementation of the second aspect, the model building module can be specifically used to equate the legs of the two-wheeled leg robot to a connecting rod, and the torso of the two-wheeled leg robot to a rigid body with mass, with the geometric center of the rigid body serving as the center of mass of the rigid body, and connecting the two ends of the connecting rod to the rigid body and the wheel feet of the two-wheeled leg robot respectively.

[0035] In a specific implementation of the second aspect, the prediction control module may include:

[0036] A matrix expression determination unit, configured to determine a discretized state transfer matrix expression of the two-wheeled leg robot according to the dynamic model expression;

[0037] A predicted state quantity determination unit is used to obtain the current state quantity of the two-wheeled leg robot and determine the predicted state quantity of the two-wheeled leg robot after a predetermined time period based on the current state quantity and the state transfer matrix expression;

[0038] a current control amount determination unit, configured to obtain an expected state amount of the two-wheeled legged robot after the predetermined time period, and optimize the control amount of the two-wheeled legged robot according to the predicted state amount and the expected state amount, to determine the current control amount of the two-wheeled legged robot;

[0039] A robot control unit is used to control the two-wheeled leg robot according to the current control value.

[0040] In a specific implementation of the second aspect, the current control amount determination unit may include:

[0041] A state quantity error calculation subunit, configured to calculate a state quantity error between the predicted state quantity and the expected state quantity;

[0042] An optimization objective function construction subunit, configured to construct an optimization objective function according to the state quantity error and the control quantity of the two-wheeled leg robot;

[0043] A quadratic programming solving subunit, configured to perform a quadratic programming solving on the optimization objective function to obtain a control variable of the two-wheeled legged robot at each control time point within the predetermined time period;

[0044] The current control amount determination subunit is used to determine the control amount at the first control time point within the predetermined time period as the current control amount of the two-wheeled leg robot.

[0045] In a specific implementation of the second aspect, the target optimization function is:

[0046]

[0047] sth j (X k )≤0,j∈{1,…,N}

[0048] Among them, U k is the control matrix, X k is the predicted state matrix, is the expected state matrix, Q is the first coefficient matrix, W is the second coefficient matrix, h j (X k ) is the linear constraint equation of N control time points within the predetermined time length.

[0049] A third aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of any one of the above-mentioned robot balance control methods are implemented.

[0050] A fourth aspect of an embodiment of the present application provides a robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any one of the above-mentioned robot balance control methods when executing the computer program.

[0051] A fifth aspect of the embodiments of the present application provides a computer program product, which, when running on a robot, enables the robot to execute the steps of any one of the above-mentioned robot balance control methods.

[0052] Compared with the prior art, the embodiments of the present application have the following beneficial effects: the embodiments of the present application simplify the models of the legs and torso of the two-wheeled leg robot, establish a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot; perform dynamic analysis on the two-wheeled leg flywheel inverted pendulum model to determine the dynamic model expression of the two-wheeled leg robot; perform model predictive control on the two-wheeled leg robot based on the dynamic model expression to maintain the balance of the two-wheeled leg robot. Through the embodiments of the present application, the established two-wheeled leg flywheel inverted pendulum model can be dynamically analyzed to obtain the dynamic model expression, and the two-wheeled leg robot can be model predictive controlled based on the model expression, thereby realizing the control quantity constraint of the two-wheeled leg robot and improving the robustness of the balance control of the two-wheeled leg robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the embodiments or descriptions of the prior art. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0054] Figure 1 This is a flow chart of an embodiment of a robot balance control method in an embodiment of the present application;

[0055] Figure 2 This is a schematic diagram of the wheel-foot flywheel inverted pendulum model;

[0056] Figure 3 This is a schematic flow chart for the dynamic analysis of the double-wheeled foot flywheel inverted pendulum model;

[0057] Figure 4 It is a schematic diagram of the force analysis of the left wheel foot;

[0058] Figure 5 Schematic diagram of the force analysis of the connecting rod;

[0059] Figure 6 Schematic diagram of force analysis on a rigid body;

[0060] Figure 7 The figure is a schematic flow chart of model predictive control for a two-wheeled legged robot;

[0061] Figure 8 is a schematic flow chart of a process for determining the current control value of a two-wheeled legged robot;

[0062] Figure 9 This is a structural diagram of an embodiment of a robot balance control device in an embodiment of the present application;

[0063] Figure 10 This is a schematic block diagram of a robot in an embodiment of the present application. DETAILED DESCRIPTION

[0064] In order to make the purpose, features, and advantages of the invention of this application more obvious and easy to understand, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the drawings in the embodiments of this application. Obviously, the embodiments described below are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0065] It will be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0066] It should also be understood that the terms used in this specification are for the purpose of describing specific embodiments only and are not intended to limit the present application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms unless the context clearly indicates otherwise.

[0067] It should be further understood that the term "and / or" used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

[0068] As used in this specification and the appended claims, the term "if" can be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrase "if it is determined" or "if [described condition or event] is detected" can be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

[0069] In addition, in the description of the present application, the terms "first", "second", "third", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0070] In existing technologies, PID or LQR algorithms are commonly used for balance control of two-wheeled robots. However, PID algorithms suffer from poor robustness, while LQR algorithms exhibit large errors and fail to account for constraints on the control variable. Therefore, there is currently a lack of a balance control algorithm for two-wheeled robots that is both robust and constrained.

[0071] In view of this, the embodiments of the present application provide a robot balance control method, device, computer-readable storage medium and robot to solve the problem that the existing balance control algorithm is difficult to be compatible with robustness and control quantity constraints of a two-wheeled legged robot.

[0072] See also Figure 1 In an embodiment of the present application, a robot balance control method may include:

[0073] Step S101: simplify the models of the legs and torso of the two-wheeled leg robot, and establish a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot.

[0074] Specifically, in the embodiment of the present application, the legs of the two-wheeled robot can be equivalent to a connecting rod, the torso of the two-wheeled robot can be equivalent to a rigid body with mass, and the two ends of the connecting rod can be connected to the rigid body and the wheel feet of the two-wheeled robot respectively.

[0075] It is understandable that the wheel foot of the two-wheeled robot includes a left wheel foot and a right wheel foot, and the left wheel foot and the right wheel foot are symmetrical structures. For the sake of convenience, the wheel foot flywheel inverted pendulum model composed of the left wheel foot of the two-wheeled robot is used as an example to illustrate the model simplification process in the embodiment of this application. Figure 2 The schematic diagram of the wheeled-legged flywheel inverted pendulum model shown above shows that the torso of the two-wheeled robot (including structures such as the thorax) can be approximated as a massive rigid body, and the legs can be approximated as connecting rods, with the legs connected to the left wheeled leg and the rigid body, respectively. Similarly, the wheeled-legged flywheel inverted pendulum model for the right wheeled leg can be derived using the above model simplification process.

[0076] Step S102: Perform dynamic analysis on the two-wheeled foot flywheel inverted pendulum model to determine the dynamic model expression of the two-wheeled foot robot.

[0077] See also Figure 3 Step S102 may include the following specific processes:

[0078] Step S1021: Perform dynamic analysis on the wheel foot, connecting rod and rigid body in the two-wheeled foot flywheel inverted pendulum model respectively to determine the dynamic equation of the two-wheeled foot robot.

[0079] In an embodiment of the present application, the two-wheeled foot flywheel inverted pendulum model is obtained by simplifying the wheel feet, legs and torso of the two-wheeled foot robot, so the wheel feet, connecting rods and rigid bodies in the two-wheeled foot flywheel inverted pendulum model can be dynamically analyzed first to determine the dynamic equation of the two-wheeled foot robot, and then the dynamic model expression can be obtained based on the obtained dynamic equation.

[0080] The following will explain the dynamic analysis process of the wheel foot, connecting rod and rigid body in the embodiment of the present application respectively.

[0081] In the embodiment of the present application, the wheel foot can be subjected to dynamic analysis. Since the left wheel foot and the right wheel foot of the two-wheeled foot robot in the embodiment of the present application are symmetrical structures, the left wheel foot is taken as an example to illustrate the dynamic analysis process of the wheel foot. In the embodiment of the present application, the geometric center of the left wheel foot can be used as the center of mass of the left wheel foot. The force on the left wheel foot mainly comes from the input of the connecting rod, the ground and the robot joint. Specifically, please refer to Figure 4 The left wheel foot force analysis diagram shown in the figure ( Figure 4 Only part of the force analysis is shown in the figure), where the force on the left wheel of the two-wheeled robot from the connecting rod can be decomposed into the force in the x direction and the force in the z direction, that is, H Lx and H Lz ; The force acting on the left wheel from the ground is F L In the embodiment of the present application, it can be considered that there is only rolling friction between the left wheel foot and the ground, and no sliding friction; G w is the gravity on the left wheel foot; θ L is the rotation angle of the left wheel foot; τ l is the joint input torque of the left wheel foot, and R is the radius of the left wheel foot.

[0082] It can be understood that, because the right wheel foot and the left wheel foot in the embodiment of the present application are symmetrical structures, the force analysis of the right wheel foot can be obtained by referring to the above-mentioned force analysis process of the left wheel foot, which will not be repeated here.

[0083] In the embodiment of the present application, the connecting rod can also be subjected to dynamic analysis. In the embodiment of the present application, the geometric center of the connecting rod can be used as the center of mass of the connecting rod. The force on the connecting rod mainly comes from the wheel foot and the rigid body, and the connecting rod will rotate relative to the wheel foot and the rigid body. Specifically, you can refer to Figure 5 The schematic diagram of connecting rod force analysis ( Figure 5 Only part of the load analysis is shown). Lx and H Lz The projections of the force on the connecting rod from the left wheel foot in the x direction and the projections of the force on the connecting rod from the right wheel foot in the x direction and the z direction can be recorded as H Rx and H Rz ( Figure 5 (not shown in the figure), the force acting on the connecting rod from the wheel foot (including the left wheel foot and the right wheel foot) can be recorded as H Wx and H Wz ,in:

[0084]

[0085] The projections of the force on the connecting rod from the rigid body in the x and z directions are H fx and H fz ; G p is the gravity acting on the connecting rod; θ p is the angle of the connecting rod relative to the vertical plane.

[0086] In the embodiment of the present application, dynamic analysis can also be performed on the rigid body. In the embodiment of the present application, the geometric center of the rigid body can be used as the center of mass of the rigid body. The rigid body is mainly affected by the force from the connecting rod and its own gravity. Figure 6 The rigid body force analysis diagram is shown in the figure, where H fx and H fz are the projections of the force on the rigid body from the connecting rod in the x and z directions respectively; θ f is the angle of the rigid body relative to the horizontal plane; G f The gravity acting on the rigid body.

[0087] In the embodiment of the present application, the kinetic equation determined after kinetic analysis may be:

[0088]

[0089] Among them, m f is the mass of the rigid body in the double-wheeled flywheel inverted pendulum model, m p is the mass of the connecting rod in the double-wheeled flywheel inverted pendulum model, L fw L is the distance between the rigid body axis and the wheel foot axis, c is the distance between the center of the rigid body and the wheel axis, is the forward acceleration of the two-wheeled robot, is the angular acceleration of the connecting rod relative to the vertical plane, J p is the moment of inertia of the rigid body around the wheel foot axis, J f is the moment of inertia of the rigid body around its axis of rotation, J w is the moment of inertia of the wheel foot around the wheel foot axis, is the angular acceleration of the rigid body relative to the horizontal plane, D is the width of the two-wheeled robot, I p is the moment of inertia of the connecting rod relative to the z-axis, is the yaw acceleration of the connecting rod, τ r is the joint input torque of the right wheel foot, τ f Enter the torques for the joints of the rigid body.

[0090] Step S1022: Determine the dynamic model expression of the two-wheeled leg robot according to the dynamic equation.

[0091] In the embodiment of the present application, the forward displacement and speed of the wheel foot of the two-wheeled robot, the angle, angular velocity, yaw angle, yaw angular velocity of the connecting rod relative to the vertical plane, the angle and angular velocity of the rigid body relative to the horizontal plane can be used as the system state quantity, and the joint input torque of the left wheel foot, the joint input torque of the right wheel foot and the joint input torque of the rigid body can be used as the control quantity to obtain the dynamic model expression. That is, it can be recorded as the system state quantity Control quantity u=[τ l ,τ r ,τ f ] T , and determine the kinetic model expression as follows:

[0092]

[0093] Among them, A is the state matrix of the two-wheeled leg robot, and B is the input matrix of the two-wheeled leg robot.

[0094] Step S103: performing model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot.

[0095] Model Predictive Control (MPC) predicts the system state in a subsequent period of time based on the current state of the system and future inputs from the current moment, and optimizes future control inputs so that the future system state can reach the expected value. The embodiment of the present application adopts the method of model predictive control for balance control, so that the two-wheeled foot flywheel inverted pendulum model can maintain the balance of torso position and posture.

[0096] See also Figure 7Step S103 may include the following specific processes:

[0097] Step S1031: Determine the discretized state transfer matrix expression of the two-wheeled leg robot according to the dynamic model expression.

[0098] In the embodiment of the present application, it is assumed that the two-wheeled foot robot is predicted to have a time interval from the current moment (denoted as k) to the predetermined time interval (denoted as T s ) after the state quantity, the number of prediction steps is N, then each short period of time within the prediction range is T s / N, where the N control time points are simplified as k+1, k+2,…, k+N moments, and the corresponding control time points are k+T s / N,k+2*T s / N,…,k+T s , a control quantity is fixed at each control time point, denoted as u k ,u k+1 ,…,u k+N-1 From the dynamic model expression, the discretized state transfer matrix expression can be determined:

[0099]

[0100] in,

[0101] Step S1032: Acquire the current state of the two-wheeled legged robot, and determine the predicted state of the two-wheeled legged robot after a predetermined time period based on the current state and the state transfer matrix expression.

[0102] In the embodiment of the present application, the current state of the two-wheeled foot robot can be recorded as x k According to the above state transfer matrix expression, the predicted state quantities of the two-wheeled foot robot at each control time point within the preset time length can be recursively predicted as follows:

[0103]

[0104] …

[0105]

[0106] The above prediction state quantities can be integrated into the form of a matrix:

[0107] X k =Ψx k +ΘU k

[0108] in:

[0109]

[0110] It should be understood that the lower triangular form in the above formula directly reflects the temporal causal relationship of the system state quantity, that is, the current system state quantity is only affected by the past input, and is not affected by the input at subsequent moments, that is, the system state quantity at time k is not affected by the input at time k+1.

[0111] Step S1033: obtaining the expected state quantity of the two-wheeled leg robot after a predetermined period of time, and optimizing the control quantity of the two-wheeled leg robot according to the predicted state quantity and the expected state quantity to determine the current control quantity of the two-wheeled leg robot.

[0112] In an embodiment of the present application, the state quantity error between the predicted state quantity and the expected state quantity can be calculated first, and then the optimization objective function can be constructed with the state quantity error and the control quantity of the two-wheeled leg robot, and the optimization objective function can be solved by quadratic programming (QP).

[0113] See also Figure 8 Step S1033 may include the following specific processes:

[0114] Step S1033a: Calculate the state quantity error between the predicted state quantity and the expected state quantity.

[0115] It is understandable that in the embodiment of the present application, the control goal is to make the system state quantity in k+T s The predicted value at time k+T tracks the expected value s The predicted state quantity at the moment is as close as possible to the expected state quantity. The smaller the error between the predicted state quantity and the expected state quantity, the closer the predicted state is to the expected state. Therefore, in the embodiment of the present application, the error between the predicted state quantity and the expected state quantity can be used as a component of the optimization objective function.

[0116] Step S1033b: construct an optimization objective function based on the state quantity error and the control quantity of the two-wheeled leg robot.

[0117] It is understandable that in order to improve resource utilization, while ensuring that the state quantity error is as small as possible, it is hoped that the control quantity of the two-wheeled leg robot can also be as small as possible. Therefore, in the embodiment of the present application, for each prediction moment, the state quantity error and the control quantity of the two-wheeled leg robot can be minimized as optimization goals and written in quadratic form:

[0118]

[0119] in, is the expected state quantity at the prediction time k, w1 is the first coefficient, w2 is the second coefficient, and its dimension corresponds to the time dimension of the state quantity and the control quantity. The above quadratic form is expanded to the entire prediction time domain, that is, considering a total of N prediction times from k+1 to k+N, the optimization objectives of the N times are integrated to form the optimization objective function f(U) for the entire prediction time domain. k ), converted into the standard form of QP:

[0120]

[0121] sth j (X k )≤0,j∈{1,…,N}

[0122] Among them, U is the predictive control quantity, U k is the control matrix, X k is the predicted state matrix, is the expected state matrix, Q is the first coefficient matrix, W is the second coefficient matrix, h j (X k ) is the linear constraint equation for the N control time points within the above-mentioned predetermined time length, and the above-mentioned matrices correspond one to one in the time dimension.

[0123] It should be noted that, in the embodiment of the present application, the constraint conditions may include the control quantity amplitude constraint and the state quantity limit constraint (including the maximum speed and position), and can be written in the form of a linear constraint equation through the state space equation, that is, the above h j (X k ).

[0124] Step S1033c: performing quadratic programming on the optimization objective function to obtain the control amount of the two-wheeled leg robot at each control time point within a predetermined time length.

[0125] By solving the above optimization problem with the QP solver, the control quantity of the two-wheeled foot robot at N control time points within a predetermined time period can be obtained, that is, U = [u k ,u k+1 ,…,u k+N-1 ] T .

[0126] Step S1033d: determine the control amount at the first control time point within the predetermined time length as the current control amount of the two-wheeled leg robot.

[0127] In the embodiment of the present application, the control amount u at the first control time point within the predetermined time period can be k Determine the current control quantity of the two-wheeled foot robot.

[0128] Step S1034: Control the two-wheeled leg robot according to the current control value.

[0129] In an embodiment of the present application, the determined current control amount of the two-wheeled leg robot can be applied to the wheel foot and the rigid body, and then at each control time point, a rolling optimization solution can be performed according to steps S1032 to S1034 to achieve optimized control of the entire process.

[0130] In summary, the embodiment of the present application simplifies the model of the legs and torso of the two-wheeled leg robot, establishes a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot; performs a dynamic analysis on the two-wheeled leg flywheel inverted pendulum model to determine the dynamic model expression of the two-wheeled leg robot; and performs model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot. Through the above method, the established two-wheeled leg flywheel inverted pendulum model can be dynamically analyzed to obtain the dynamic model expression, and the two-wheeled leg robot can be model predictive controlled according to the model expression, thereby realizing the control quantity constraint of the two-wheeled leg robot and improving the robustness of the balance control of the two-wheeled leg robot.

[0131] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0132] Corresponding to the robot balance control method described in the above embodiment, Figure 9 The figure shows a structural diagram of an embodiment of a robot balance control device provided in an embodiment of the present application, which is applied to a two-wheeled and legged robot.

[0133] In this embodiment, a robot balance control device applied to a two-wheeled robot may include:

[0134] A model building module 901 is used to simplify the models of the legs and torso of the two-wheeled leg robot and build a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot;

[0135] A dynamic model determination module 902 is used to perform dynamic analysis on the two-wheeled foot flywheel inverted pendulum model to determine a dynamic model expression of the two-wheeled foot robot;

[0136] The predictive control module 903 is used to perform model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot.

[0137] In a specific implementation of an embodiment of the present application, the model building module can be specifically used to treat the legs of the two-wheeled leg robot as equivalent to a connecting rod, treat the torso of the two-wheeled leg robot as equivalent to a rigid body with mass, use the geometric center of the rigid body as the center of mass of the rigid body, and connect the two ends of the connecting rod to the rigid body and the wheel feet of the two-wheeled leg robot respectively.

[0138] In a specific implementation of the embodiment of the present application, the dynamic model determination module may include:

[0139] a dynamic equation determination unit, configured to perform dynamic analysis on the wheel foot, connecting rod, and rigid body in the two-wheeled foot flywheel inverted pendulum model respectively, and determine the dynamic equation of the two-wheeled foot robot;

[0140] The model expression determination unit is used to determine the dynamic model expression of the two-wheeled leg robot according to the dynamic equation.

[0141] In a specific implementation of the embodiment of the present application, the kinetic equation is:

[0142]

[0143] Among them, m f is the mass of the rigid body in the two-wheeled flywheel inverted pendulum model, m p is the mass of the connecting rod in the double-wheeled flywheel inverted pendulum model, L fw L is the distance between the rigid body axis and the wheel foot axis, c is the distance between the center of mass of the rigid body and the rotation axis of the wheel foot, is the forward acceleration of the two-wheeled foot robot, θ p is the angle of the connecting rod relative to the vertical plane, is the angular acceleration of the connecting rod relative to the vertical plane, J p is the moment of inertia of the rigid body around the wheel foot axis, J w is the moment of inertia of the wheel foot around the wheel foot axis, J f is the moment of inertia of the rigid body around the rigid body axis, is the angular acceleration of the rigid body relative to the horizontal plane, R is the radius of the wheel foot, D is the width of the two-wheeled foot robot, I p is the moment of inertia of the connecting rod relative to the z-axis, is the yaw acceleration of the connecting rod, τ l is the joint input torque of the left wheel foot, τ r is the joint input torque of the right wheel foot, τ f Enter the torques for the joints of the rigidbody.

[0144] In a specific implementation of the embodiment of the present application, the prediction control module may include:

[0145] A matrix expression determination unit, configured to determine a discretized state transfer matrix expression of the two-wheeled leg robot according to the dynamic model expression;

[0146] A predicted state quantity determination unit is used to obtain the current state quantity of the two-wheeled leg robot and determine the predicted state quantity of the two-wheeled leg robot after a predetermined time period based on the current state quantity and the state transfer matrix expression;

[0147] a current control amount determination unit, configured to obtain an expected state amount of the two-wheeled legged robot after the predetermined time period, and optimize the control amount of the two-wheeled legged robot according to the predicted state amount and the expected state amount, to determine the current control amount of the two-wheeled legged robot;

[0148] A robot control unit is used to control the two-wheeled leg robot according to the current control value.

[0149] In a specific implementation of the embodiment of the present application, the current control amount determination unit may include:

[0150] A state quantity error calculation subunit, configured to calculate a state quantity error between the predicted state quantity and the expected state quantity;

[0151] An optimization objective function construction subunit, configured to construct an optimization objective function according to the state quantity error and the control quantity of the two-wheeled leg robot;

[0152] A quadratic programming solving subunit, configured to perform a quadratic programming solving on the optimization objective function to obtain a control variable of the two-wheeled legged robot at each control time point within the predetermined time period;

[0153] The current control amount determination subunit is used to determine the control amount at the first control time point within the predetermined time period as the current control amount of the two-wheeled leg robot.

[0154] In a specific implementation of the embodiment of the present application, the objective optimization function is:

[0155]

[0156] sth j (X k )≤0,j∈{1,…,N}

[0157] Among them, U k is the control matrix, X k is the predicted state matrix, is the expected state matrix, Q is the first coefficient matrix, W is the second coefficient matrix, h j (X k ) is the linear constraint equation of N control time points within the predetermined time length.

[0158] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described devices, modules and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0159] In the above embodiments, the description of each embodiment has its own focus. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant description of other embodiments.

[0160] Figure 10 A schematic block diagram of a robot provided in an embodiment of the present application is shown. For ease of explanation, only the parts related to the embodiment of the present application are shown.

[0161] like Figure 10 As shown, the robot 10 of this embodiment includes: a processor 100, a memory 101, and a computer program 102 stored in the memory 101 and executable on the processor 100. When the processor 100 executes the computer program 102, the steps in the above-mentioned embodiments of the robot balance control method are implemented, such as Figure 1 Alternatively, when the processor 100 executes the computer program 102, the functions of the modules / units in the above-mentioned device embodiments are realized, for example, Figure 9 Functions of modules 901 to 903 are shown.

[0162] For example, the computer program 102 may be divided into one or more modules / units, which are stored in the memory 101 and executed by the processor 100 to implement the present application. The one or more modules / units may be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program 102 in the robot 10.

[0163] Those skilled in the art will understand that Figure 10 This is merely an example of the robot 10 and does not constitute a limitation of the robot 10. The robot 10 may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the robot 10 may also include input and output devices, network access devices, buses, etc.

[0164] The processor 100 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0165] The memory 101 may be an internal storage unit of the robot 10, such as a hard drive or memory of the robot 10. The memory 101 may also be an external storage device of the robot 10, such as a plug-in hard drive, a Smart Media Card (SMC), a Secure Digital (SD) card, a flash memory card, etc. equipped on the robot 10. Furthermore, the memory 101 may include both an internal storage unit of the robot 10 and an external storage device. The memory 101 is used to store the computer program and other programs and data required by the robot 10. The memory 101 may also be used to temporarily store data that has been output or is about to be output.

[0166] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.

[0167] In the above embodiments, the description of each embodiment has its own focus. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant description of other embodiments.

[0168] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0169] In the embodiments provided in this application, it should be understood that the disclosed devices / robots and methods can be implemented in other ways. For example, the device / robot embodiments described above are merely schematic. For example, the division of the modules or units is merely a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0170] The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected to achieve the purpose of this embodiment according to actual needs.

[0171] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0172] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present application implements all or part of the process in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and when the computer program is executed by the processor, it can implement the steps of the above-mentioned various method embodiments. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable storage medium may include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable storage media do not include electric carrier signals and telecommunication signals.

[0173] The above-described embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present application, and should all be included in the scope of protection of the present application.

Claims

1. A robot balance control method, characterized in that: Applied to a two-wheeled foot robot, the method includes: Simplifying the models of the legs and torso of the two-wheeled leg robot and establishing a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot; Determine the dynamic equation of the two-wheeled leg robot: Among them, m f is the mass of the rigid body in the two-wheeled flywheel inverted pendulum model, m p is the mass of the connecting rod in the double-wheeled flywheel inverted pendulum model, L fw L is the distance between the rigid body axis and the wheel foot axis, c is the distance between the center of mass of the rigid body and the rotation axis of the wheel foot, is the forward acceleration of the two-wheeled foot robot, θ p is the angle of the connecting rod relative to the vertical plane, is the angular acceleration of the connecting rod relative to the vertical plane, J p is the moment of inertia of the rigid body around the wheel foot axis, J w is the moment of inertia of the wheel foot around the wheel foot axis, J f is the moment of inertia of the rigid body around the rigid body's axis of rotation, is the angular acceleration of the rigid body relative to the horizontal plane, R is the radius of the wheel foot, D is the width of the two-wheeled foot robot, I p is the moment of inertia of the connecting rod relative to the z-axis, is the yaw acceleration of the connecting rod, τ l is the joint input torque of the left wheel foot, τ r is the joint input torque of the right wheel foot, τ f Input torque to the joints of the rigid body; determine the dynamic model expression of the two-wheeled leg robot according to the dynamic equation; Model predictive control is performed on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot.

2. The robot balance control method according to claim 1, characterized in that: In the two-wheeled flywheel inverted pendulum model of the two-wheeled robot, the legs of the two-wheeled robot are equivalent to a connecting rod, the torso of the two-wheeled robot is equivalent to a rigid body with mass, the geometric center of the rigid body serves as the center of mass of the rigid body, and the two ends of the connecting rod are respectively connected to the rigid body and the wheel feet of the two-wheeled robot.

3. The robot balance control method according to any one of claims 1 to 2, characterized in that: Performing model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot includes: Determine a discretized state transfer matrix expression of the two-wheeled leg robot according to the dynamic model expression; Acquire a current state quantity of the two-wheeled leg robot, and determine a predicted state quantity of the two-wheeled leg robot after a predetermined time period according to the current state quantity and the state transfer matrix expression; Obtaining an expected state quantity of the two-wheeled leg robot after the predetermined time period, and optimizing and controlling a control quantity of the two-wheeled leg robot according to the predicted state quantity and the expected state quantity to determine a current control quantity of the two-wheeled leg robot; The two-wheeled leg robot is controlled according to the current control amount.

4. The robot balance control method according to claim 3, characterized in that: The step of optimizing the control amount of the two-wheeled leg robot according to the predicted state amount and the expected state amount to determine the current control amount of the two-wheeled leg robot includes: Calculating a state quantity error between the predicted state quantity and the expected state quantity; Constructing an optimization objective function according to the state quantity error and the control quantity of the two-wheeled foot robot; Performing a quadratic programming solution on the optimization objective function to obtain a control variable of the two-wheeled leg robot at each control time point within the predetermined time period; The control amount at the first control time point within the predetermined time period is determined as the current control amount of the two-wheeled leg robot.

5. The robot balance control method according to claim 4, characterized in that: The optimization objective function is: s.t.h j (X k )≤0,j∈{1,…,N} Among them, U k is the control matrix, X k is the predicted state matrix, is the expected state matrix, Q is the first coefficient matrix, W is the second coefficient matrix, h j (X k ) is the linear constraint equation of N control time points within the predetermined time length.

6. A robot balance control device, characterized in that: Applied to a two-wheeled foot robot, the device comprises: A model building module is used to simplify the models of the legs and torso of the two-wheeled leg robot and establish a two-wheeled leg flywheel inverted pendulum model of the two-wheeled leg robot; The dynamic model determination module is used to determine the dynamic equation of the two-wheeled leg robot: Among them, m f is the mass of the rigid body in the two-wheeled flywheel inverted pendulum model, m p is the mass of the connecting rod in the double-wheeled flywheel inverted pendulum model, L fw L is the distance between the rigid body axis and the wheel foot axis, c is the distance between the center of mass of the rigid body and the rotation axis of the wheel foot, is the forward acceleration of the two-wheeled foot robot, θ p is the angle of the connecting rod relative to the vertical plane, is the angular acceleration of the connecting rod relative to the vertical plane, J p is the moment of inertia of the rigid body around the wheel foot axis, J w is the moment of inertia of the wheel foot around the wheel foot axis, J f is the moment of inertia of the rigid body around the rigid body's axis of rotation, is the angular acceleration of the rigid body relative to the horizontal plane, R is the radius of the wheel foot, D is the width of the two-wheeled foot robot, I p is the moment of inertia of the connecting rod relative to the z-axis, is the yaw acceleration of the connecting rod, τ l is the joint input torque of the left wheel foot, τ r is the joint input torque of the right wheel foot, τ f Input torque to the joints of the rigid body; determine the dynamic model expression of the two-wheeled leg robot according to the dynamic equation; A predictive control module is used to perform model predictive control on the two-wheeled leg robot according to the dynamic model expression to maintain the balance of the two-wheeled leg robot.

7. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the robot balance control method according to any one of claims 1 to 5 are implemented.

8. A robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the robot balance control method according to any one of claims 1 to 5 are implemented.

Images