Humanoid robot predictive control method with redundancy freedom degree constraint and robot

Abstract

The invention provides a humanoid robot predictive control method based on redundancy degree of freedom constraint and a robot. The method comprises the steps of creating a first dynamic model of a biped lower limb system in the humanoid robot based on product parameters of the humanoid robot; according to the first dynamical model and a reference trajectory planned based on the motion task of the humanoid robot, control parameters of the biped lower limb system walking along the reference trajectory and corresponding control costs are predicted, and the control costs comprise the state cost and the input cost of the biped lower limb system; based on the control parameters, a cost function in a quadratic programming form is created, and the cost function comprises redundant joint constraint terms of the humanoid robot; determining an optimal control parameter of the humanoid robot based on the cost function and a first constraint condition with a priority corresponding to the motion task; and joint motors in the humanoid robot are controlled to operate based on the optimal control parameters. Therefore, the problem that two feet of the humanoid robot are difficult to stably run in a complex environment can be solved.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a humanoid robot predictive control method with redundant freedom constraints and the robot. Background Art

[0002] In order to adapt to human society and complete various tasks in complex and unknown environments on behalf of humans, humanoid robots need to have the ability of their bipedal lower limb system to achieve balanced and stable movement. Improving the walking stability and adaptability of the bipedal lower limb system in real environment scenarios is an open problem worthy of in-depth research. In addition, in order to meet the needs of humanoid robot motion adaptability in different scenarios, redundant degrees of freedom are sometimes added to the calf or ankle joints of the bipedal lower limb system. How to properly handle such redundant degrees of freedom when the robot is working also puts higher requirements on the robustness of the control algorithm. At present, the traditional control algorithm of the bipedal lower limb system faces the problem of difficulty in coping with complex motion tasks in complex environments. Summary of the invention

[0003] In view of this, the purpose of the embodiments of the present application is to provide a humanoid robot predictive control method and robot with redundant degrees of freedom constraints, which can improve the problem that humanoid robots have difficulty in coping with complex motion tasks in complex environments.

[0004] In order to achieve the above technical objectives, the technical solutions adopted in this application are as follows:

[0005] In a first aspect, an embodiment of the present application provides a method for predictive control of a humanoid robot with redundant degrees of freedom constraints, the method comprising:

[0006] Based on the product parameters of the humanoid robot, a first dynamic model of a bipedal lower limb system in the humanoid robot is created;

[0007] According to the first dynamic model and the reference trajectory planned based on the motion task of the humanoid robot, predicting the control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory, wherein the control cost includes the state cost and input cost of the bipedal lower limb system;

[0008] Based on the control parameters, creating a cost function in a quadratic programming form, wherein the cost function includes redundant joint constraint terms of the humanoid robot;

[0009] Determining optimal control parameters of the humanoid robot based on the cost function and a first constraint condition with priority corresponding to the motion task;

[0010] The operation of the joint motors in the humanoid robot is controlled based on the optimal control parameters.

[0011] In combination with the first aspect, in some optional implementations, based on product parameters of the humanoid robot, a first dynamics model of a bipedal lower limb system in the humanoid robot is created, including:

[0012] According to the dynamic model of the multi-joint linkage system, based on the parameters corresponding to the floating base and leg linkage structure of the humanoid robot in the product parameters, an underactuated decoupling model is created;

[0013] Among them, the dynamic model of the multi-joint connecting rod system is:

[0014]

[0015] Where M represents the mass matrix, C is the Coriolis force and centrifugal force matrix, G is the gravity matrix, τ is the input torque, and θ is the joint angle;

[0016] The underactuated decoupling model is:

[0017]

[0018] Where M u represents the mass matrix of the robot; q and v are the generalized joint angles and velocities of the humanoid robot, respectively; b u represents the Coriolis force and centripetal force matrix of the humanoid robot; J cu represents the contact force Jacobian matrix of the humanoid robot, F c represents the superimposed contact force, and the subscript u represents underactuation;

[0019] According to the Newton-Euler formula, a dynamic differential equation is constructed for the underactuated decoupling model, and a first dynamic model of the biped lower limb system is created. The dynamic differential equation is:

[0020]

[0021] The first kinetic model is:

[0022]

[0023] Where x refers to the state variable of the bipedal lower limb system, Θ refers to the Euler angle of the bipedal lower limb system; p c refers to the three-dimensional spatial coordinates of the bipedal lower limb system; w refers to the angular velocity of the bipedal lower limb system; refers to the linear velocity of the bipedal lower limb system; g refers to the gravity constant; A refers to the system matrix; B refers to the input matrix; u refers to the input variable of the bipedal lower limb system, u=[f1 f2τ1v2] T; f1 refers to the contact force between the left foot and the ground in the bipedal lower limb system; f2 refers to the contact force between the right foot and the ground in the bipedal lower limb system; τ1 refers to the torque of the left foot in the bipedal lower limb system; v2 refers to the torque of the right foot in the bipedal lower limb system;

[0024]

[0025]

[0026] Where, ψ is the angle of the bipedal lower limb system around the z-axis; m is the body mass of the bipedal lower limb system; I 3×3 represents the identity matrix; 0 3×3 represents the zero matrix; p1 represents the spatial position vector of the left foot; p2 represents the spatial position vector of the right foot, p c A spatial position vector representing the center of mass of the bipedal lower limb system.

[0027] In combination with the first aspect, in some optional implementations, before predicting the control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory according to the first dynamics model and the reference trajectory planned based on the motion task of the humanoid robot, the method further includes:

[0028] Based on the second constraint, a control cost function J of the biped lower limb system is created, which is expressed as:

[0029]

[0030] In the formula, (x i+1 -x i+1,r ) T Q i (x i+1 -x i+1,r ) represents the state cost of the bipedal lower limb system, represents the input cost of the bipedal lower limb system; x i+1 is the current motion state of the bipedal lower limb system, x i+1,r is the reference trajectory for planning; Q i is the weight matrix of the state cost, R i is the weight matrix of the input cost, i represents the number of times in the accumulation process, k-1 is the upper limit of the accumulation number; the subscript r represents the planned data;

[0031] The second constraint is:

[0032]

[0033] Among them, F min ≤F iz ≤Fmax is the normal force F iz The upper and lower limit constraints; v xmin ≤τ ix ≤τ xmax , τ ymin ≤τ iy ≤τ ymax , τ ymin ≤τ iy ≤τ ymax , τ zmin ≤τ iz ≤τ zmax The upper and lower limit constraints of the three-dimensional moment of the foot end in the bipedal lower limb system; iz ≤F ix ≤μF iz and -μF iz ≤F iy ≤μF iz is the foot end friction cone constraint in the biped lower limb system, μ refers to the friction coefficient; the foot marks x, y, z represent the directions in three-dimensional space;

[0034] The control cost function is converted into an expression based on a quadratic programming problem:

[0035]

[0036] Wherein, U, F, H are all obtained through the system matrix A, input matrix B, weight matrix Q and weight matrix R, and are all parameter matrices related to the system matrix A, input matrix B, weight matrix Q and weight matrix R.

[0037] In combination with the first aspect, in some optional implementations, predicting control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory according to the first dynamics model and a reference trajectory planned based on the motion task of the humanoid robot includes:

[0038] Inputting the reference trajectory and the current motion state of the biped lower limb system obtained based on the first dynamic model into the control cost function to obtain a control cost;

[0039] Creating a second dynamics model representing a floating base of the humanoid robot based on the control cost, and obtaining the control parameter based on the second dynamics model;

[0040] Wherein, the second kinetic model is:

[0041]

[0042] Where M(q) is the generalized mass matrix, which is a positive definite symmetric matrix; q, and represent the generalized joint position, generalized joint velocity and generalized joint acceleration of the floating base respectively; represents the Coriolis force term; g(q) represents the gravitational constant term; and F ext Represent the contact Jacobian and contact force respectively; τ represents the driving torque of the joint motor; S refers to the selection matrix of the joint motor, and the selection matrix is ​​a diagonal matrix;

[0043]

[0044] The control parameters include the input parameters u of the floating base b , the input parameter u of the joint motor j .

[0045] In combination with the first aspect, in some optional implementations, creating a cost function in a quadratic programming form based on the control parameter includes:

[0046] Create a cost function D in quadratic programming form, expressed as:

[0047]

[0048] In the formula, J i represents the Jacobian matrix of the ith item, represents the derivative of the Jacobian matrix of the ith term, represents the joint acceleration in the humanoid robot, represents the joint velocities in the humanoid robot, represents the desired acceleration in the humanoid robot, α i represents the weight coefficient of the i-th item; x cg represents the current position of the center of mass of the humanoid robot, x cg,ref represents the reference trajectory of the center of mass, φ represents the current angle of the redundant joints in the humanoid robot, and φ ref represents the reference angle of the redundant joint, and the control parameters include x cg 、x cg,ref , φ, and φ ref , Refers to the redundant joint constraint item.

[0049] In combination with the first aspect, in some optional implementations, the first constraint condition is as follows, with descending priority:

[0050]

[0051] τ m ≤τ≤τ M

[0052] In the formula, Jc,j represents the Jacobian matrix, represents the derivative of the Jacobian matrix, and Represent joint velocity and joint acceleration respectively; U j is the friction constraint, λ j is the Lagrange multiplier; Represent the functions related to joint angle and angular velocity respectively.

[0053] In combination with the first aspect, in some optional implementations, determining the optimal control parameters of the humanoid robot based on the cost function and a first constraint condition with priority corresponding to the motion task includes:

[0054] Under the first constraint condition, the control parameter corresponding to the minimum cost function D is used as the optimal control parameter, and the optimal control parameter includes the output acceleration of the joint motor in the humanoid robot.

[0055]

[0056] In the formula, is the expected acceleration, v is the current speed feedback value, x is the motor angle feedback value, v d 、x d Represent the expected speed and expected position respectively, K d , K p They are the differential control gain and the proportional control gain respectively.

[0057] In a second aspect, an embodiment of the present application further provides a robot, comprising a processor and a memory coupled to each other, wherein a computer program is stored in the memory, and when the computer program is executed by the processor, the robot executes the above method.

[0058] In combination with the second aspect, in some optional embodiments, the robot is a humanoid robot with redundant joints.

[0059] The invention adopting the above technical solution has the following advantages:

[0060] In the technical solution provided in the present application, the control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory are predicted by using the first dynamic model of the bipedal lower limb system in the humanoid robot and the reference trajectory planned based on the motion task of the humanoid robot; then, based on the control parameters, a cost function in the form of quadratic programming is created, and the cost function and the first constraint condition with priority corresponding to the motion task are used to determine the optimal control parameters of the humanoid robot; finally, the optimal control parameters are used to control the operation of the joint motors in the humanoid robot. In this way, by predicting the control parameters of the bipedal lower limb system in the future short-term state and performing predictive optimization control, the problem that the humanoid robot is difficult to stably travel with its two feet in a complex environment can be improved. In addition, by prioritizing the constraints, it is beneficial to achieve balance control of the humanoid robot under the common constraints of multiple types of parameters. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The present application may be further described by the non-limiting embodiments given in the accompanying drawings. It should be understood that the following drawings only illustrate certain embodiments of the present application and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, other relevant drawings may be obtained based on these drawings without creative effort.

[0062] Figure 1 A schematic diagram of the structure of a bipedal lower limb system in a robot provided in an embodiment of the present application.

[0063] Figure 2 A flowchart of a predictive control method for a humanoid robot with redundant degrees of freedom constraints provided in an embodiment of the present application.

[0064] Figure 3 A logic diagram of the predictive control of a bipedal lower limb system provided in an embodiment of the present application.

[0065] Figure 4 A schematic diagram of the dynamic modeling of the floating base of the bipedal lower limb system provided in an embodiment of the present application.

[0066] Figure 5 A schematic diagram of the structure of the actual platform of the humanoid robot provided in the embodiment of the present application.

[0067] Figure 6 A schematic diagram of the motion states of a simulation platform of a humanoid robot provided in an embodiment of the present application.

[0068] Icons: 10-biped lower limb system; 11-thigh rotation joint motor; 12-hip joint motor; 13-knee joint motor; 14-ankle joint motor; 15-calf rotation joint motor; 16-floating base; 17-fixed base. DETAILED DESCRIPTION

[0069] The present application will be described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that in the drawings or descriptions, similar or identical parts use the same figure numbers, and the implementation methods not shown or described in the drawings are forms known to ordinary technicians in the relevant technical field. In the description of this application, the terms "first", "second", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0070] The present application provides a robot that may include a processor and a memory. The memory stores a computer program, and when the computer program is executed by the processor, the robot is able to perform the corresponding steps in the following redundant freedom constraint humanoid robot predictive control method.

[0071] In this embodiment, the robot may be a humanoid robot with redundant joints, and includes a biped lower limb system 10 .

[0072] Please refer to Figure 1 , Figure 1 (a) is a front view of the bipedal lower limb system 10, Figure 1 (b) is a side view of the bipedal lower limb system 10. The bipedal lower limb system 10 has a thigh swivel joint, a hip joint, a knee joint, an ankle joint and a calf swivel joint. In addition, the bipedal lower limb system 10 is provided with corresponding joint motors for driving the corresponding joint parts to move. For example, the bipedal lower limb system 10 includes a thigh swivel joint motor 11, a hip joint motor 12, a knee joint motor 13, an ankle joint motor 14 and a calf swivel joint motor 15, and the joint motors can be used to drive the humanoid robot to walk and adjust the walking posture. Among them, the redundant joints can provide redundant degrees of freedom. As an example, the redundant joints can be calf swivel joints.

[0073] Please refer to Figures 2 to 6 The present application also provides a humanoid robot predictive control method with redundant degrees of freedom constraints, referred to as the control method, which can be applied to the above-mentioned robot / humanoid robot. The control method optimizes the control based on the model predictive control strategy of the dynamic model constraints, and comprehensively handles multiple types of constraints such as redundant degrees of freedom, joint restrictions, ground friction, and body collision by using the priority sorting and loose optimization method of the whole body collaborative control strategy, which expands and enriches the practical application of model predictive control theory in the bipedal lower limb system.

[0074] Please refer to Figure 2 , the control method may include the following steps:

[0075] Step 110, creating a first dynamics model of a biped lower limb system in the humanoid robot based on product parameters of the humanoid robot;

[0076] Step 120, predicting control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory according to the first dynamics model and the reference trajectory planned based on the motion task of the humanoid robot, wherein the control cost includes a state cost and an input cost of the bipedal lower limb system;

[0077] Step 130, creating a cost function in a quadratic programming form based on the control parameters, wherein the cost function includes redundant joint constraint terms of the humanoid robot;

[0078] Step 140, determining optimal control parameters of the humanoid robot based on the cost function and a first constraint condition with priority corresponding to the motion task;

[0079] Step 150: Control the operation of the joint motors in the humanoid robot based on the optimal control parameters.

[0080] The following will describe the various steps of the control method in detail, as follows:

[0081] In this embodiment, the hardware structure of the bipedal lower limb system can be divided into two parts: the waist core and the legs. The waist core mainly includes a core controller, a battery, and various control and communication submodule hardware. The legs mainly include two 7-DOF redundant connecting rod structures, which are detailed from the waist down as 3-DOF hip joints and connecting rods, 1-DOF knee joints and connecting rods, 1-DOF calf (yaw redundant freedom) and connecting rods, and 2-DOF ankle joints and connecting rods. In order to facilitate algorithm design and actual machine debugging, under the premise of meeting various basic movements of the bipedal lower limb system (flat ground, rugged roads, ramps, steps), the calf rotation joint is used as a general redundant joint for model predictive control modeling, and the special posture balance task is added as a new target task to the whole body collaborative control. The waist core mass in the bipedal lower limb system is large, but there is no driven joint, which is a typical under-driven type. The legs are joint-connecting rod structures, which are typical full-drive types. In order to describe the dynamic characteristics of the bipedal lower limb system, it is necessary to decouple the two (the waist and legs of the robot) for modeling.

[0082] In step 110, based on the product parameters of the humanoid robot, a first dynamic model of the bipedal lower limb system in the humanoid robot is created, including:

[0083] According to the multi-joint linkage system dynamics model, based on the parameters corresponding to the floating base 16 and the leg linkage structure of the humanoid robot in the product parameters, an underactuated decoupling model is created. Specifically, the product parameters may include, but are not limited to, the mass of the humanoid robot, the length of the linkage, etc.;

[0084] Among them, the dynamic model of the multi-joint connecting rod system is:

[0085]

[0086] Where M represents the mass matrix, C is the Coriolis force and centrifugal force matrix, G is the gravity matrix, τ is the input torque, and θ is the joint angle;

[0087] The underactuated decoupling model is:

[0088]

[0089] Where M u represents the mass matrix of the robot; q and v are the generalized joint angles and velocities of the humanoid robot, respectively; b u representing the Coriolis force and centripetal force matrices of the humanoid robot; represents the contact force Jacobian matrix of the humanoid robot, F c represents the superimposed contact force, and the subscript u represents underactuation;

[0090] The full drive decoupling model is:

[0091]

[0092] In the formula, the subscript a represents full drive. Usually, the center of mass of the bipedal lower limb system is concentrated on the floating base 16. Therefore, the underactuated decoupling model can be used as the main control model to realize the construction of a single rigid body model after the bipedal lower limb system is decoupled. Based on this, the underactuated decoupling model is constructed according to the Newton-Euler formula to construct a dynamic differential equation, and the first dynamic model of the bipedal lower limb system is created. The dynamic differential equation is:

[0093]

[0094] Incorporating the gravity constant term g into the state variable vector, the first dynamic model in linear standard form can be obtained as:

[0095]

[0096] Where x refers to the state variable of the bipedal lower limb system, Θ refers to the Euler angle (posture) of the bipedal lower limb system; p crefers to the three-dimensional spatial coordinates (spatial position) of the bipedal lower limb system; w refers to the angular velocity of the bipedal lower limb system; refers to the linear velocity of the bipedal lower limb system; g refers to the gravity constant; A refers to the system matrix; B refers to the input matrix; u refers to the input variable of the bipedal lower limb system, u=[f1 f2τ1τ2] T ; f1 refers to the contact force between the left foot and the ground in the bipedal lower limb system; f2 refers to the contact force between the right foot and the ground in the bipedal lower limb system; τ1 refers to the torque of the left foot in the bipedal lower limb system; τ2 refers to the torque of the right foot in the bipedal lower limb system;

[0097]

[0098] Where, ψ is the angle of the bipedal lower limb system around the z-axis; m is the body mass of the bipedal lower limb system; I 3×3 represents the identity matrix; 0 3×3 represents the zero matrix; p1 represents the spatial position vector of the left foot; p2 represents the spatial position vector of the right foot, p c A spatial position vector representing the center of mass of the bipedal lower limb system.

[0099] When the structure and motion model of the humanoid robot are fixed, in the bipedal lower limb system, the system matrix A and the input matrix B can be calculated using the posture data obtained by the IMU (Inertial Measurement Unit) and the mass of the machine body.

[0100] Before step 120, the method may further include:

[0101] Based on the second constraint, a control cost function J of the biped lower limb system is created, which is expressed as:

[0102]

[0103] In the formula, (x i+1 -x i+1,r ) T Q i (x i+1 -x i+1,r ) represents the state cost of the bipedal lower limb system, represents the input cost of the bipedal lower limb system; x i+1 is the current motion state of the bipedal lower limb system, x i+1,r is the reference trajectory for planning; Q i is the weight matrix of the state cost, R i is the weight matrix of the input cost, i represents the number of times in the accumulation process, k-1 is the upper limit of the accumulation number; the subscript r represents the planned data;

[0104] Based on the basic idea of ​​model predictive control, the state cost and input cost of the system are optimized to the minimum under the premise of system constraints, that is:

[0105]

[0106] The second constraint is:

[0107]

[0108] Among them, F min ≤F iz ≤F max is the normal force F iz The upper and lower limit constraints of τ xmin ≤τ ix ≤τ xmax , τ ymin ≤τ iy ≤τ ymax , τ ymin ≤τ iy ≤τ ymax , τ zmin ≤τ iz ≤τ zmax The upper and lower limit constraints of the three-dimensional moment of the foot end in the bipedal lower limb system; iz ≤F ix ≤μF iz and -μF iz ≤F iy ≤μF iz is the foot end friction cone constraint in the biped lower limb system, μ refers to the friction coefficient, and the footmarks x, y, z represent the directions in three-dimensional space; the second constraint condition is mainly for the legs of the biped lower limb system;

[0109] The control cost function is converted into an expression based on a quadratic programming problem:

[0110]

[0111] Wherein, U, F, H are all obtained through the system matrix A, input matrix B, weight matrix Q and weight matrix R, and are all parameter matrices related to the system matrix A, input matrix B, weight matrix Q and weight matrix R.

[0112] In step 120, the control parameters of the bipedal lower limb system walking along the reference trajectory can be predicted using the prediction model, and the prediction model is the following formula (13) and (14). The calculation process of the model predictive control faces the simultaneous optimization of the system state and input. If the direct method (considering both the state quantity and the control quantity as decision variables) is used and the nonlinear programming solver is used for solving, there will be a risk of control signal update delay and system crash caused by the large computational burden. Therefore, it is converted into a quadratic programming problem as shown in the above formula (12). For this quadratic programming problem, the solver can be used to solve its real-time numerical solution. The operating logic diagram of the predictive control algorithm of the bipedal lower limb system is as follows: Figure 3 As shown. Among them, U, F, and H are parameter matrices related to the weight matrix Q, R, the system matrix A, and the input matrix B, which can be calculated based on the set Q, R and the pre-calculated A, B.

[0113] In this embodiment, by using the control cost function J to perform real-time optimization solution, a set of contact torque sequences that meet the planning trajectory requirements and basic swing leg constraints, plantar torque, friction cone and other restrictions can be obtained. The basic movement of the bipedal lower limb system is achieved by controlling the motor drive of each joint. Generally speaking, for leg-foot robots with low joint complexity and light leg mass, such as quadruped robots, the virtual work principle can be used to solve the torque of each joint, that is, the inverse solution process of the plantar torque based on statics. However, the bipedal lower limb system has many joints and the leg mass cannot be ignored, and the solution of statics in the high-dynamic movement process cannot meet the actual performance requirements of the robot. Therefore, in this application, a multi-rigid body dynamics solution method based on a floating base 16 is introduced to solve the torque of each joint of the robot.

[0114] Due to the under-actuated characteristics of the bipedal lower limb robot, the control of the motors in the legs cannot directly affect the robot's upper limb system, that is, the robot's upper limb system is in a semi-determined "floating" state. The floating block at the waist concentrates the main mass of the bipedal lower limb system, which will have a huge impact on the overall balance and stability of the system when the robot performs various types of tasks. Therefore, in order to be able to control and analyze the floating base 16 of the robot, this application introduces a virtual six-degree-of-freedom series structure to connect the floating base 16 to the fixed base 17, such as Figure 4 shown.

[0115] Step 120, predicting control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory according to the first dynamics model and the reference trajectory planned based on the motion task of the humanoid robot, includes:

[0116] Inputting the reference trajectory and the current motion state of the biped lower limb system obtained based on the first dynamic model into the control cost function to obtain a control cost;

[0117] Based on the control cost, creating a second dynamic model representing the floating base 16 of the humanoid robot, and obtaining the control parameter based on the second dynamic model;

[0118] Wherein, the second kinetic model is:

[0119]

[0120] Where M(q) is the generalized mass matrix, which is a positive definite symmetric matrix; q, and Respectively represent the generalized joint position, generalized joint velocity and generalized joint acceleration of the floating base 16; represents the Coriolis force term; g(q) represents the gravitational constant term; and F ext Represent the contact Jacobian and contact force respectively; τ represents the driving torque of the joint motor, and the selection matrix S of the joint motor is added; S refers to the selection matrix of the joint motor, the selection matrix is ​​a diagonal matrix, and the selection coefficient corresponding to the virtual six-degree-of-freedom series mechanism is 0; based on this, the selection matrix can extract the driving joint from the joint vector of the complete floating base 16, which is expressed as:

[0121]

[0122] The control parameters include the input parameters u of the floating base 16 b , the input parameter u of the joint motor j For the multi-rigid body dynamics model of the floating base 16, it is not possible to calculate all joint torques by only knowing the joint accelerations. It is necessary to calculate the linear acceleration and angular acceleration of the floating base 16 through external forces, that is, to calculate the generalized acceleration matrix first, so that the driving torque of each joint can be calculated using formula (13).

[0123] In this embodiment, the optimal foot contact force sequence u can be solved by the prediction model using step 120. Based on the optimal foot contact force sequence u and the floating base 16 multi-rigid body dynamics equation formula (13), the balanced and stable motion control of the bipedal lower limb system can be achieved. However, the bipedal lower limb system faces the needs of different levels of task objectives during movement, such as maintaining balance, maintaining posture, following the desired trajectory, following the swing leg trajectory, etc. At the same time, in order to improve the stability of the robot when walking on special terrains (such as steps and ramps), it is necessary to independently plan the walking stability task for the yaw redundant joints of the calf (calf rotation joints). For the same joint torque input interface, in order to meet the multi-level task constraints as much as possible, it is necessary to prioritize each type of control objective.

[0124] In this application, the optimization idea of ​​sequential quadratic programming is introduced to perform hierarchical solutions to optimization tasks at different levels. For example, step 130, based on the control parameters, creates a cost function in the form of quadratic programming, which may include:

[0125] Create a cost function D in quadratic programming form, expressed as:

[0126]

[0127] In the formula, J i represents the Jacobian matrix of the ith item, represents the derivative of the Jacobian matrix of the ith term, represents the joint acceleration in the humanoid robot, represents the joint velocities in the humanoid robot, represents the desired acceleration in the humanoid robot, α i represents the weight coefficient of the i-th item; x cg represents the current position of the center of mass of the humanoid robot, x cg,ref represents the reference trajectory of the center of mass, φ represents the current angle of the redundant joints in the humanoid robot, and φ ref represents the reference angle of the redundant joint, and the control parameters include x cg 、x cg,ref , φ, and φ ref ;

[0128] Refers to the dynamic constraints of the humanoid robot, Refers to the center of mass constraint of the humanoid robot, Refers to the redundant joint constraint item.

[0129] The cost function in the quadratic programming form is subject to the first constraint. The constraints in the first constraint clause have decreasing priorities from formula (15a) to formula (15e). Specifically, the constraints in the first constraint clause are as follows:

[0130]

[0131]

[0132] τ m ≤τ≤τ M (15e)

[0133] In the formula, J c,j represents the Jacobian matrix, represents the derivative of the Jacobian matrix, and Represent joint velocity and joint acceleration respectively; U j is the friction constraint, λ j is the Lagrange multiplier; Respectively represent the functions related to the joint angle and angular velocity;

[0134] Formula (15a) represents the floating basis dynamics constraint of the biped lower limb system as the optimization task with the first priority;

[0135] Formula (15b) represents the contact point acceleration limit, which means that the acceleration of the robot's sole in contact with the ground is required to be 0 during movement, ensuring that it is not easy to move relative to the ground.

[0136] Formula (15c) represents the contact point friction cone limit, which ensures the stability of the robot contact point friction force and avoids sliding or rolling. j is the friction constraint, λ j is the Lagrange multiplier, which is non-zero only when it touches the ground;

[0137] Formula (15d) represents the joint acceleration limit, They represent functions related to joint angle and angular velocity, respectively, and represent the upper and lower limits of acceleration. The joint acceleration is integrated to ensure that the speed and angle under this acceleration motion do not exceed the joint limit.

[0138] Formula (15e) represents the joint torque limit, which ensures that the joint control torque does not exceed the maximum and minimum torque limits of the joint.

[0139] In the quadratic programming form of the cost function D, joint acceleration, joint torque and external friction are all decision variables. The cost function D maps the joint space variables to the Cartesian space through the Jacobian matrix, constraining its acceleration to follow the target acceleration setting value, and the task weight is weighted by the scalar value.

[0140] In step 140, based on the cost function and a first constraint condition with a priority corresponding to the motion task, determining the optimal control parameters of the humanoid robot includes:

[0141] Under the first constraint condition, the control parameter corresponding to the minimum cost function D is used as the optimal control parameter, and the optimal control parameter includes the output acceleration of the joint motor in the humanoid robot.

[0142]

[0143] In the formula, is the expected acceleration, v is the current speed feedback value, x is the motor angle feedback value, v d 、x d Represent the expected speed and expected position respectively, K d , K p They are the differential control gain and the proportional control gain respectively.

[0144] In this embodiment, the output acceleration of each joint motor of the humanoid robot is It is calculated by the task trajectory in Cartesian space through a feedforward PD (proportional differential) controller through the expected position, expected velocity and expected acceleration, as shown in the above formula (16). d , K p , the most suitable gain can be selected through actual machine debugging.

[0145] In step 150, the humanoid robot can use the optimal control parameters obtained in step 140 to control the speed, acceleration and other parameters of the corresponding joint motors, so that the robot can stably perform complex motion tasks in a complex environment.

[0146] In order to verify the actual control effect of this method, the inventor built a real machine platform of a humanoid robot, such as Figure 5As shown. To ensure the basic algorithm strategy development space, the bipedal lower limb system is equipped with a control core i7-1360P that can run a real-time operating system and high-performance computing capabilities. To ensure fast and stable communication of multi-joint actuators, the CANopen communication protocol based on the CAN bus architecture is adopted. To meet the general requirements of the body shape of humanoid robots, the bipedal lower limb system is designed with a double leg upright height of 951 mm. Due to its high center of gravity and non-minimum phase characteristics, excessive mass will increase the difficulty of control and is not conducive to the universal and simplified design concept of robots. Therefore, its overall weight after removing the power supply is controlled at 28 kg. The overall degree of freedom (DoF) of the legs is 14. While having the basic humanoid robot load walking ability, the redundant calf joints can be combined to fine-tune the yaw center of gravity. In addition, in order to meet the large load and high dynamic motion goals of the bipedal lower limb system, the hip joint torque is ≥400Nm and the joint rated speed is ≥360° / s.

[0147] To ensure the effectiveness and safety of the algorithm, a high-fidelity simulation platform for the bipedal lower limb system was built in the Mujoco environment to conduct environmental interaction tests on the above control algorithm. Based on the established mechanical structure, sensor, and actuator basic indicators of the physical platform of the bipedal lower limb system, the urdf file was used to describe the physical kinematics of the bipedal lower limb system, including: 1) connecting rod properties, such as connecting rod geometry and material properties; 2) joint properties, such as connecting rod connection mode, degree of freedom, and joint limit; 3) coordinate system relationship, which is used to define the subordinate relationship between each joint and connecting rod; 4) kinematic physical properties, such as mass, inertia, friction coefficient, etc.; 5) dynamic characteristics, such as gravity and damping, etc.

[0148] In addition, the mjcf file is used to continue the basic description of the interaction between the bipedal lower limb system and the environment. In addition to the robot-related kinematic and dynamic properties corresponding to the urdf, it is also necessary to set the environmental properties such as the geometric characteristics of the ground and obstacles, as well as the robot's joint position, speed, force and IMU sensor, so as to maximize the authenticity of the robot's interaction with the environment in the simulation environment. Figure 6 shown.

[0149] In order to verify the feasibility, rationality and effectiveness of the technology proposed in this paper, experiments were carried out on the simulation platform Mujoco and the real machine platform.

[0150] (1) A bipedal lower limb system motion control test space was established in the Mujoco environment. It includes a flat ground, 10 cm high steps, and up and down ramps with a slope of 5°. At the same time, in order to increase the harshness of the environment, the robot directly descends to the uphill position of the ramp after ascending the four steps. After giving a forward speed of 1m / s, the robot will pass through the flat ground, steps, and ramp terrain in turn. From the experimental results, it can be seen that the robot can walk smoothly under various types of terrain, showing strong motion stability and robustness.

[0151] (2) By monitoring the center of mass position, velocity, acceleration, and joint angles and torques of the lower limb system during movement, it can be seen that the plantar feedback force of the robot can be kept within the limit range throughout the entire movement process. At the same time, thanks to the redundant yaw joints in the lower leg, the robot can make fine adjustments in direction while maintaining relatively stable overall movements when walking on a slope, and the center of mass does not shift seriously, which is conducive to improving the robot's ability to adapt to complex terrain.

[0152] In this embodiment, the object of predictive control is the bipedal lower limb system, through the laboratory environment hoisting single joint control test, multi-joint linkage squat sideways test and whole machine walking test. During the test, the angle, speed and torque of each joint are monitored in real time, and the IMU signal of the body is displayed as a curve. It can be seen that the robot can ensure that the joint torque is within the limit during the entire movement process, and the foothold and the ground remain without relative displacement during walking, and the body can remain relatively stable without large-scale and high-frequency side deviation.

[0153] In this embodiment, multiple types of constraint optimization are added to the output end of the multi-contact force sequence of the model predictive control, including 1) the physical characteristics of the robot body are constrained by the dynamics of the floating base 16 to achieve dynamic tracking of the position, velocity, and acceleration of the floating base; 2) the robot contact point restriction constrained by the contact point acceleration and the contact friction cone to achieve the constraint of no sliding of the supporting leg and no rollover of the center of mass during the robot's stepping process; 3) the joint motor restriction constrained by the maximum and minimum angles, velocities, accelerations, and torque inequalities of each driving joint to avoid the control target torque output by the control algorithm exceeding the capacity range of the joint motor; 4) the independent yaw task constraint of the redundant joint of the lower leg to achieve fine-tuning of the robot leg movement during complex terrain movement and ensure the relative stability of the center of mass of the whole machine. The above types of constraints are sorted according to the task priority during the movement of the bipedal lower limb system, and are summarized as quadratic programming problems for hierarchical solution optimization.

[0154] A floating base 16 dynamic modeling method is designed for the bipedal lower limb system. This method introduces virtual serial joints to transform the floating base 16 dynamic problem into a fixed base 17 dynamic problem, which more accurately expresses the physical motion characteristics of the bipedal lower limb system and improves the stability and robustness of the optimization control algorithm restricted by this model when facing different types of terrain environments.

[0155] Based on the above design, this scheme conducts whole-body dynamics modeling of the bipedal lower limb system and introduces a model predictive control strategy to predict and optimize the future short-term state of the bipedal lower limb system, which can solve the problem that bipedal balance stability is difficult to cope with unknown interference in complex environments.

[0156] In addition, by utilizing the priority sorting and loose optimization characteristics of whole-body motion control, the multi-constraint optimization of the joint torque of the bipedal lower limb system can solve the problem of balance control under the joint constraints of multiple types of parameters such as friction changes and random collisions that traditional motion control strategies of bipedal lower limbs with redundant degrees of freedom are difficult to cope with. This is conducive to the stable movement of the robot in complex environments (uncertain interference constraints such as friction changes and random collisions).

[0157] In this embodiment, the processor in the robot can be, but is not limited to, a central processing unit (CPU), a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, and can implement or execute the methods, steps and logic block diagrams disclosed in the embodiments of the present application.

[0158] The memory may be, but is not limited to, a random access memory, a read-only memory, a programmable read-only memory, an erasable programmable read-only memory, an electrically erasable programmable read-only memory, etc. In this embodiment, the storage module may be used to store the first constraint condition, the second constraint condition, the dynamics model, etc. Of course, the storage module may also be used to store a program, and the processing module executes the program after receiving the execution instruction.

[0159] Through the description of the above implementation methods, technical personnel in this field can clearly understand that the present application can be implemented by hardware, and can also be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present application can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.), including a number of instructions for enabling a computer device (which can be a personal computer, a robot, or a network device, etc.) to execute the methods described in each implementation scenario of the present application.

[0160] In the embodiments provided in the present application, it should be understood that the disclosed robot, system and method can also be implemented in other ways. The robot, system and method embodiments described above are merely schematic, for example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the systems, methods and computer program products according to the multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a part of a module, a program segment or a code, and a part of the module, program segment or code contains one or more executable instructions for implementing the specified logical function. It should also be noted that each box in the block diagram and / or the flowchart, and the combination of the boxes in the block diagram and / or the flowchart can be implemented by a dedicated hardware-based system that performs the specified function or action, or can be implemented by a combination of dedicated hardware and computer instructions. In addition, each functional module in each embodiment of the present application can be integrated together to form an independent part, or each module can exist separately, or two or more modules can be integrated to form an independent part.

[0161] The above description is only an embodiment of the present application and is not intended to limit the protection scope of the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A predictive control method for a humanoid robot with redundant degrees of freedom constraints, characterized in that: The method comprises: Based on the product parameters of the humanoid robot, a first dynamic model of a bipedal lower limb system in the humanoid robot is created; According to the first dynamic model and the reference trajectory planned based on the motion task of the humanoid robot, predicting the control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory, wherein the control cost includes the state cost and input cost of the bipedal lower limb system; Based on the control parameters, creating a cost function in a quadratic programming form, wherein the cost function includes redundant joint constraint terms of the humanoid robot; Determining optimal control parameters of the humanoid robot based on the cost function and a first constraint condition with priority corresponding to the motion task; The operation of the joint motors in the humanoid robot is controlled based on the optimal control parameters.

2. The method according to claim 1, characterized in that Based on the product parameters of the humanoid robot, a first dynamic model of a biped lower limb system in the humanoid robot is created, including: According to the dynamic model of the multi-joint linkage system, based on the parameters corresponding to the floating base and leg linkage structure of the humanoid robot in the product parameters, an underactuated decoupling model is created; Among them, the dynamic model of the multi-joint connecting rod system is: Where M represents the mass matrix, C is the Coriolis force and centrifugal force matrix, G is the gravity matrix, τ is the input torque, and θ is the joint angle; The underactuated decoupling model is: Where M u represents the mass matrix of the robot; q and v are the generalized joint angles and velocities of the humanoid robot, respectively; b u representing the Coriolis force and centripetal force matrices of the humanoid robot; represents the contact force Jacobian matrix of the humanoid robot, F c represents the superimposed contact force, and the subscript u represents underactuation; According to the Newton-Euler formula, a dynamic differential equation is constructed for the underactuated decoupling model, and a first dynamic model of the biped lower limb system is created. The dynamic differential equation is: The first kinetic model is: Where x refers to the state variable of the bipedal lower limb system, Θ refers to the Euler angle of the bipedal lower limb system; p c refers to the three-dimensional spatial coordinates of the bipedal lower limb system; w refers to the angular velocity of the bipedal lower limb system; refers to the linear velocity of the bipedal lower limb system; g refers to the gravity constant; A refers to the system matrix; B refers to the input matrix; u refers to the input variable of the bipedal lower limb system, u=[f1f2τ1τ2] T ; f1 refers to the contact force between the left foot and the ground in the bipedal lower limb system; f2 refers to the contact force between the right foot and the ground in the bipedal lower limb system; τ1 refers to the torque of the left foot in the bipedal lower limb system; τ2 refers to the torque of the right foot in the bipedal lower limb system; Where, ψ is the angle of the bipedal lower limb system around the z-axis; m is the body mass of the bipedal lower limb system; I 3×3 represents the identity matrix; 0 3×3 represents the zero matrix; p1 represents the spatial position vector of the left foot; p2 represents the spatial position vector of the right foot, p c A spatial position vector representing the center of mass of the bipedal lower limb system.

3. The method according to claim 2, characterized in that Before predicting the control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory according to the first dynamics model and the reference trajectory planned based on the motion task of the humanoid robot, the method further includes: Based on the second constraint, a control cost function J of the biped lower limb system is created, which is expressed as: In the formula, (x i+1 -x i+1,r ) T Q i (x i+1 -x i+1,r ) represents the state cost of the bipedal lower limb system, represents the input cost of the bipedal lower limb system; x i+1 is the current motion state of the bipedal lower limb system, x i+1,r is the planned reference trajectory; Q i is the weight matrix of the state cost, R i is the weight matrix of the input cost, i represents the number of times in the accumulation process, k-1 is the upper limit of the accumulation number; the subscript r represents the planned data; The second constraint is: Among them, F min ≤F iz ≤F max is the normal force F iz The upper and lower limit constraints of τ xmin ≤τ ix ≤τ xmax , τ ymin ≤τ iy ≤τ ymax , τ ymin ≤τ iy ≤τ ymax , τ zmin ≤τ iz ≤τ zmax The upper and lower limit constraints of the three-dimensional moment of the foot end in the bipedal lower limb system; iz ≤F ix ≤μF iz and -μF iz ≤F iy ≤μF iz is the foot end friction cone constraint in the biped lower limb system, μ refers to the friction coefficient; the foot marks x, y, z represent the directions in three-dimensional space; The control cost function is converted into an expression based on a quadratic programming problem: Wherein, U, F, H are all obtained through the system matrix A, input matrix B, weight matrix Q and weight matrix R, and are all parameter matrices related to the system matrix A, input matrix B, weight matrix Q and weight matrix R.

4. The method according to claim 3, characterized in that According to the first dynamic model and the reference trajectory planned based on the motion task of the humanoid robot, predicting the control parameters and corresponding control costs of the bipedal lower limb system walking along the reference trajectory includes: Inputting the reference trajectory and the current motion state of the biped lower limb system obtained based on the first dynamic model into the control cost function to obtain a control cost; Creating a second dynamics model representing a floating base of the humanoid robot based on the control cost, and obtaining the control parameter based on the second dynamics model; Wherein, the second kinetic model is: Where M(q) is the generalized mass matrix, which is a positive definite symmetric matrix; q, and represent the generalized joint position, generalized joint velocity and generalized joint acceleration of the floating base respectively; represents the Coriolis force term; g(q) represents the gravitational constant term; and F ext Represent the contact Jacobian and contact force respectively; τ represents the driving torque of the joint motor; S refers to the selection matrix of the joint motor, and the selection matrix is ​​a diagonal matrix; The control parameters include the input parameters u of the floating base b , the input parameter u of the joint motor j .

5. The method according to claim 4, characterized in that Based on the control parameters, a cost function in the form of quadratic programming is created, including: Create a cost function D in quadratic programming form, expressed as: In the formula, J i represents the Jacobian matrix of the ith item, represents the derivative of the Jacobian matrix of the ith term, represents the joint acceleration in the humanoid robot, represents the joint velocities in the humanoid robot, represents the desired acceleration in the humanoid robot, α i represents the weight coefficient of the i-th item; x cg represents the current position of the center of mass of the humanoid robot, x cg,ref represents the reference trajectory of the center of mass, φ represents the current angle of the redundant joints in the humanoid robot, and φ ref represents the reference angle of the redundant joint, and the control parameters include x cg 、x cg,ref , φ, and φ ref , Refers to the redundant joint constraint item.

6. The method according to claim 5, characterized in that The first constraints are as follows, in descending order of priority: t m ≤τ≤τ M In the formula, J c,j represents the Jacobian matrix, represents the derivative of the Jacobian matrix, and Represent joint velocity and joint acceleration respectively; U j is the friction constraint, λ j is the Lagrange multiplier; Represent the functions related to joint angle and angular velocity respectively.

7. The method according to claim 6, characterized in that Determining optimal control parameters of the humanoid robot based on the cost function and a first constraint condition with priority corresponding to the motion task, comprising: Under the first constraint condition, the control parameter corresponding to the minimum cost function D is used as the optimal control parameter, and the optimal control parameter includes the output acceleration of the joint motor in the humanoid robot. In the formula, is the expected acceleration, v is the current speed feedback value, x is the motor angle feedback value, v d 、x d Represent the expected speed and expected position respectively, K d , K p They are the differential control gain and the proportional control gain respectively.

8. A robot, characterized in that: The robot comprises a processor and a memory coupled to each other, wherein a computer program is stored in the memory. When the computer program is executed by the processor, the robot executes the method according to any one of claims 1 to 7.

9. The robot according to claim 8, characterized in that: The robot is a humanoid robot with redundant joints.

Images