Whole body control method and device for legged robot combined with real-time kinematic prediction

Abstract

The application relates to a full-body control method and device of a legged robot combined with real-time kinematic prediction, wherein the method comprises the following steps: generating a task space reference motion trajectory of a full-body motion task of the legged robot in a future preset time length, which is decomposed into multiple sub-motion tasks; acquiring a current state of the legged robot, and respectively calculating each component of the task space reference motion trajectory of each sub-motion task by using a preset real-time kinematic prediction (RKP) algorithm according to the current state to obtain multiple expected accelerations; and delivering the multiple expected accelerations to a full-body control (WBC) to coordinate the multiple sub-motion tasks until the legged robot ends the full-body motion task. Therefore, the problems that the existing control method of the legged robot requires a huge amount of calculation, the control algorithm cannot achieve a higher operation frequency, and the control performance of the robot is difficult to improve are solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, in particular to a full-body control method and device of a legged robot combined with real-time kinematic prediction. BACKGROUND

[0002] The current common control methods of legged robots can be mainly divided into four categories: simplified model method, complete model method, hybrid model method and biological mechanism method.

[0003] Among them, the simplified model method abstracts the legged robot into a simple model that is easy to analyze, and controls the legged robot according to the dynamic characteristics of the simplified model. This model method includes: Linear Inverted Pendulum Model (LIPM) control method, "three-part control" method implemented using Spring Loaded Inverted Pendulum (SLIP) model, active impedance control method, model predictive control (MPC) method using simplified model, etc.

[0004] The complete model method models the legged robot as a model composed of multiple rigid links and joint nodes, and thus designs a suitable control method according to the complete dynamic model. This includes: Whole Body Control (WBC) method, MPC method using complete model, etc.

[0005] The hybrid model method uses different dynamic models under different motion states of the legged robot, such as: Hybrid Zero Dynamics (HZD) control method, etc.

[0006] The biological mechanism method is a control method designed according to the biological mechanism of legged animals, which makes the legged robot move according to the gait pattern of legged animals, such as: Central Pattern Generator (CPG) based control method, etc.

[0007] Among the above methods, MPC can consider the reference trajectory and system state in the future for a period of time, and exhibits excellent control effect, but due to the huge amount of calculation, in order to ensure the real-time operation of MPC, only the simplified model is usually considered in MPC. Although WBC lacks consideration of future states, it can handle the complete robot motion equation at the current time, therefore, the control method combining MPC and WBC is gradually becoming the mainstream control method of legged robots.

[0008] The MPC method is to solve a discrete-time finite-time optimal control problem according to the planned motion trajectory for a limited time starting from the current time and the current robot state in each control frame, obtain the optimal control sequence for a future period of time, and execute the first frame of control quantity in the sequence. The optimal control problem is an optimization problem with equality constraints such as the robot dynamics model and other inequality constraints. In 2018, MIT researchers used a single rigid body model in MPC and performed linearization, making the constructed optimization problem a quadratic programming (QP) problem, so that real-time control can be achieved, and the MPC running frequency can reach 30 Hz. In addition to control, MPC can also be used for motion planning, and each time the MPC problem is solved, the optimized motion trajectory for a future period of time is obtained.

[0009] The WBC method.

[0010] When a robot has many degrees of freedom, in order to make the robot perform appropriate motion, multiple motion tasks are usually planned for the robot in the operation space at the same time, but due to the kinematic and dynamic limitations of the robot itself, the above tasks may not be well completed at the same time, and sometimes there may even be contradictions between some tasks. Therefore, the WBC method can be used to prioritize the above tasks, important or high-priority tasks should be completed as much as possible, and unimportant or low-priority tasks should be completed as much as possible without affecting high-priority tasks. The WBC method is widely used in various types of robots, and the foot-type robot is a typical application. In 2016, researchers used a WBC method with strict task priority hierarchy on an electrically driven four-legged robot ANYmal, achieving natural adaptability when walking on uneven terrain without vision and other perception sensors and without special planning methods.

[0011] The WBC method does not consider the future trajectory, only the current robot state and the current desired trajectory. The implementation methods of WBC mainly include two categories: the Null Space Projection (NSP) method and the optimization-based method. Here, only the optimization-based method related to the technology of this paper is introduced. WBC can be implemented by solving a single QP problem or by solving multiple QP problems. The former method is to construct a QP problem that considers the robot motion equation, joint angle, torque limit and other constraints as much as possible to complete the tasks expressed in the cost function. These tasks adjust their relative importance through a weight matrix, so this method is also called the Weighted QP (WQP) method. The latter method is to place different priority tasks (and constraints) in different QP problems, first solve the QP problem of high-priority tasks, and then solve the QP problem of low-priority tasks. When solving the QP problem of low-priority tasks, the optimal solution of the QP problem of low-priority tasks must be found in the optimal solution set of the QP problem of high-priority tasks. This approach is also called the Hierarchical QP (HQP) method. The latter can achieve strict task priority layering that the former cannot achieve, but the computational requirements of the latter are much higher than those of the former.

[0012] At present, the MPC method and the WBC method have been widely used in the control of legged robots due to their outstanding control effect. The method combining the two has gradually become the mainstream control method of legged robots. However, the computational requirements of MPC are huge, and the dimension of its optimization variables is proportional to the number of predicted frames. Even if only the state equation of the linear system is considered, the final QP problem solving may still consume a lot of time. The motion equation of the legged robot is usually nonlinear, and if the complete robot motion equation is considered in MPC, a non-convex optimization problem needs to be solved, which requires a large amount of calculation. Therefore, a simplified (approximate) robot model is usually selected in MPC to reduce the computational requirements.

[0013] Although the complete robot motion equation is considered in WBC, it can only consider the reference trajectory at the current time. The optimal solution obtained by the optimization-based WBC method is only the optimal solution for the current time, not the optimal solution for a period of time in the future. WBC cannot re-optimize the reference trajectory like MPC, so WBC often requires the planner to provide a reasonable and smooth reference trajectory.

[0014] Therefore, the related art selects a method of combining MPC and WBC, calculates MPC considering a simplified model first, and then uses WBC considering a complete model to correct the control quantity. Thus, not only the reference trajectory in the future period of time can be considered, but also the complete robot motion equation is considered, so that the control effect is obviously improved, however, the two optimization-based methods in series increase the requirement for calculation efficiency.

[0015] In summary, the control method of the legged robot in the related art has a huge calculation requirement, so that the control algorithm cannot achieve a higher running frequency, and it is difficult to show better control performance, which needs to be solved urgently. SUMMARY

[0016] The present application provides a legged robot full-body control method and device combined with real-time kinematic prediction to solve the problem that the control method of the existing legged robot has a huge calculation requirement, so that the control algorithm cannot achieve a higher running frequency, and it is difficult to improve the control performance of the robot.

[0017] The first aspect of the present application provides a legged robot full-body control method combined with real-time kinematic prediction, comprising the following steps: generating a task space reference motion trajectory of a plurality of sub-motion tasks decomposed from a full-body motion task of a legged robot in a future preset time length; obtaining a current state of the legged robot, and respectively calculating each component of the task space reference motion trajectory of each sub-motion task using a preset real-time kinematic prediction RKP algorithm according to the current state to obtain a plurality of expected accelerations; and transmitting the plurality of expected accelerations to a full-body control WBC to coordinate the plurality of sub-motion tasks until the legged robot ends the full-body motion task.

[0018] Optionally, in an embodiment of the present application, the expression of the optimization problem of the preset RKP algorithm is:

[0019]

[0020]

[0021] wherein, are the task space coordinates, velocities and accelerations of the kth frame, respectively; is a reference state trajectory in the future N frames; N is a prediction horizon; δt is the time interval of the frame; is a decision variable vector composed of task space accelerations in the entire prediction horizon; is a positive definite diagonal matrix, representing a weighted trajectory tracking error, is a positive number, which is the norm of the weighted system input.

[0022] Optionally, in an embodiment of the present application, the transmitting the plurality of desired accelerations to the whole-body control WBC to coordinate the plurality of sub-movement tasks comprises: solving an optimal solution of accelerations and foot force spinors; and based on the optimal solution of accelerations and foot force spinors, calculating corresponding control instructions according to a type of a robot effector, and controlling the robot effector to execute the control instructions.

[0023] Optionally, in an embodiment of the present application, the generating the task space reference movement trajectory of the whole-body movement task of the legged robot decomposed into a plurality of sub-movement tasks within a preset time length in the future comprises: decomposing the in-place periodic squatting movement task of the legged robot into a double-foot keeping and ground static sub-movement task and a trunk movement sub-movement task.

[0024] Optionally, in an embodiment of the present application, the generating the task space reference movement trajectory of the whole-body movement task of the legged robot decomposed into a plurality of sub-movement tasks within a preset time length in the future comprises: generating an up-down movement trajectory along a Z-axis direction, a front-back movement trajectory along an X-axis direction, a pitch angle movement trajectory rotating around a Y-axis, and a movement trajectory keeping an initial state in other three degrees of freedom of the trunk movement sub-movement task; and generating a trajectory keeping stable contact with the ground of the double-foot keeping and ground static sub-movement task.

[0025] The second aspect embodiment of the present application provides a legged robot whole-body control method combined with real-time kinematic prediction, comprising: a generating module configured to generate a task space reference movement trajectory of a whole-body movement task of a legged robot decomposed into a plurality of sub-movement tasks within a preset time length in the future; a calculating module configured to obtain a current state of the legged robot, and calculate each component of the task space reference movement trajectory of each sub-movement task respectively by using a preset real-time kinematic prediction RKP algorithm according to the current state, to obtain a plurality of desired accelerations; and a coordinating module configured to transmit the plurality of desired accelerations to a whole-body control WBC to coordinate the plurality of sub-movement tasks until the legged robot ends the whole-body movement task.

[0026] Optionally, in an embodiment of the present application, an expression of an optimization problem of the preset RKP algorithm is as follows:

[0027]

[0028]

[0029] wherein, is a task space coordinate, velocity and acceleration of the kth frame, respectively; is a reference state trajectory of future N frames; N is a prediction horizon; δt is a time interval of a frame; is a decision variable vector, composed of task space acceleration in the whole prediction horizon; is a positive definite diagonal matrix, representing a weighted trajectory tracking error, is a positive number, being a norm of a weighted system input.

[0030] Optionally, in an embodiment of the present application, the coordination module comprises: a solving unit, configured to solve the optimal solution of the acceleration and the foot force spinor; and a control unit, configured to calculate corresponding control instructions according to the type of the robot actuator based on the optimal solution of the acceleration and the foot force spinor, and control the robot actuator to execute the control instructions.

[0031] Optionally, in an embodiment of the present application, the generation module comprises: a decomposition unit, configured to decompose the sit-to-stand periodic motion task of the legged robot into a double-foot-keeping-still-to-ground sub-motion task and a trunk motion sub-motion task.

[0032] Optionally, in an embodiment of the present application, the generation module further comprises: a first keeping unit, configured to generate an up-and-down motion trajectory along the Z-axis direction, a forward-and-backward motion trajectory along the X-axis direction, a pitch angle motion trajectory rotating around the Y-axis, and motion trajectories keeping initial states in other three degrees of freedom of the trunk motion sub-motion task; and a second keeping unit, configured to generate a trajectory keeping stable contact with the ground of the double-foot-keeping-still-to-ground sub-motion task.

[0033] The third aspect embodiment of the present application provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the full-body control method of the legged robot combined with real-time kinematic prediction as described in the above embodiments.

[0034] The fourth aspect embodiment of the present application provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to implement the full-body control method of the legged robot combined with real-time kinematic prediction as described above.

[0035] Therefore, the embodiments of the present application have the following beneficial effects:

[0036] Embodiments of the present application can generate a task space reference motion trajectory of a full-body motion task of a legged robot decomposed into a plurality of sub-motion tasks within a future preset time length; obtain a current state of the legged robot, and calculate each component of the task space reference motion trajectory of each sub-motion task using a preset real-time kinematics prediction RKP algorithm according to the current state, to obtain a plurality of expected accelerations; and deliver the plurality of expected accelerations to a whole body control WBC to coordinate the plurality of sub-motion tasks until the legged robot ends the full-body motion task. The present application processes complex robot motion equations and inequality constraints using WBC, considers a reference trajectory in a future period of time using RKP (Real-time Kinematics Prediction), and re-optimizes the reference trajectory to obtain a more suitable motion trajectory. The present application is especially suitable for processing a reference trajectory with jumps, and RKP has a closed-form solution, which can effectively improve the solving efficiency, improve the running frequency, greatly reduce the calculation amount, and compensate for the defects of WBC. Thus, the problems of the existing control method of the legged robot, such as huge calculation amount requirement, inability of the control algorithm to achieve a higher running frequency, and difficulty in improving the control performance of the robot, are solved.

[0037] Additional aspects and advantages of the present application will be made apparent by the following description and the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0038] The above and / or additional aspects and advantages of the present application will become apparent and be readily appreciated from the following description, including the accompanying drawings, wherein:

[0039] Figure 1 A flowchart of a legged robot whole body control method combined with real-time kinematics prediction according to an embodiment of the present application;

[0040] Figure 2 An execution logic schematic diagram of a legged robot whole body control method combined with real-time kinematics prediction according to an embodiment of the present application;

[0041] Figure 3 An example diagram of a legged robot whole body control device combined with real-time kinematics prediction according to an embodiment of the present application;

[0042] Figure 4 A structural schematic diagram of an electronic device according to an embodiment of the present application.

[0043] Among them, 10-leg robot whole body control device combined with real-time kinematics prediction, 101-generation module, 102-computation module, 103-coordination module, 401-memory, 402-processor, 403-communication interface. DETAILED DESCRIPTION

[0044] Embodiments of the present application are described below in detail with reference to the accompanying drawings, wherein the same or similar components or components having the same or similar functions are denoted by the same or similar reference numerals throughout. The embodiments described below by reference to the accompanying drawings are exemplary and are intended to explain the present application, and cannot be understood as limiting the present application.

[0045] The full-body control method and device of a legged robot combined with real-time kinematic prediction are described below with reference to the accompanying drawings. In view of the problems mentioned in the background art, the present application provides a full-body control method of a legged robot combined with real-time kinematic prediction, in which the task space reference motion trajectory of a future preset time length is generated by decomposing a full-body motion task of a legged robot into multiple sub-motion tasks; the current state of the legged robot is obtained, and each component of the task space reference motion trajectory of each sub-motion task is calculated by using a preset real-time kinematic prediction (RKP) algorithm according to the current state, to obtain multiple expected accelerations; the multiple expected accelerations are transmitted to a whole body control (WBC) to coordinate the multiple sub-motion tasks until the legged robot ends the full-body motion task. The present application processes complex robot motion equations and inequality constraints by using WBC, considers the reference trajectory in the future by using RKP, and optimizes the reference trajectory again to obtain a more suitable motion trajectory, which is especially suitable for processing a reference trajectory with jumps. Moreover, RKP has a closed-form solution, which can effectively improve the solving efficiency, improve the running frequency, greatly reduce the calculation amount, and make up for the defects of WBC. Thus, the problems of the existing control method of a legged robot, such as huge calculation amount requirement, inability of a control algorithm to achieve a higher running frequency, and difficulty in improving the control performance of the robot, are solved.

[0046] Specifically, Figure 1 A flowchart of the full-body control method of a legged robot combined with real-time kinematic prediction provided by an embodiment of the present application is shown in FIG. 1.

[0047] As Figure 1 shown, the full-body control method of a legged robot combined with real-time kinematic prediction includes the following steps:

[0048] In step S101, a task space reference motion trajectory of a future preset time length is generated by decomposing a full-body motion task of a legged robot into multiple sub-motion tasks.

[0049] Those skilled in the art can understand that the embodiments of the present application consider the reference trajectory of a legged robot in the future while combining a real-time control algorithm of a complete robot model to achieve full-body control of the legged robot.

[0050] In order to make the skilled person more clearly understand the flow of the whole body motion control of the foot-type robot, the embodiment of the present application takes the in-place periodic squatting motion of a biped robot as an example to introduce in detail the working principle of the whole body control of the foot-type robot combined with real-time kinematic prediction.

[0051] Before controlling the foot-type robot to perform the whole body motion, the embodiment of the present application can decompose the whole body motion task into a plurality of sub-motion tasks, and the task decomposition process is not realized by an algorithm, but is performed by a user of the whole body control method of the foot-type robot according to the overall motion state of the robot that he / she expects the robot to realize, thereby providing a basis and guidance for generating the spatial reference motion trajectory corresponding to the plurality of sub-tasks.

[0052] Optionally, in an embodiment of the present application, generating the task space reference motion trajectory of the whole body motion task of the foot-type robot decomposed into a plurality of sub-motion tasks within a future preset time length comprises: decomposing the in-place periodic squatting motion task of the foot-type robot into a double-foot keeping static contact with the ground sub-motion task and a trunk motion sub-motion task.

[0053] Specifically, when performing task decomposition, the embodiment of the present application must ensure that the dimension of all tasks is greater than or equal to the total degree of freedom of the robot. For example, a biped robot with only a trunk and legs has 6 passive degrees of freedom of the trunk and 5 driven joints on each leg, so the entire robot has 6+5+5=16 degrees of freedom.

[0054] Further, the embodiment of the present application can divide the in-place squatting motion into a trunk motion and a double-foot stable contact with the ground two sub-tasks, wherein the dimension of the trunk motion sub-task is 6-dimensional, and the dimension of the double-foot stable contact with the ground sub-task is 12-dimensional (6-dimensional for each foot plate), so the dimension of all tasks is greater than the total degree of freedom of the robot, i.e., both tasks have used up all the degrees of freedom of the robot; if the dimension of all tasks is less than the total degree of freedom of the robot, it will result in redundant degrees of freedom of the robot, i.e., the overall motion of the robot is not completely specified, which will result in that the motion result of the robot can not be predictable, and therefore more tasks can be set for the robot with more redundant degrees of freedom.

[0055] It should be noted that through the above task decomposition, the motion reference trajectory is generated for the robot, and usually only the floating base (for a general biped robot, the trunk can be regarded as a floating base) and the end of the actuator (such as the end of the foot, hand, etc.) need the reference motion trajectory, so the general sub-tasks are the motion tasks of the floating base and the end of the actuator.

[0056] In addition, some other cases of tasks can also be set, for example, a task of minimizing the foot force moment and a task of minimizing the joint angular velocity, etc., which do not need to refer to the trajectory and are only set from the perspective of saving energy, etc.

[0057] It should be noted that the embodiment of the present application can set a task of minimizing the joint acceleration and the foot force moment as a regular term of the optimization problem according to actual conditions.

[0058] Therefore, the embodiment of the present application provides a basis for subsequent generation of the spatial reference motion trajectory by decomposing the whole body motion task into multiple subtasks, thereby ensuring the implementation of the whole body control of the biped robot.

[0059] Optionally, in an embodiment of the present application, the task space reference motion trajectory of the biped robot is generated by decomposing the whole body motion task into multiple subtasks within a future preset time length, including: generating the up-down motion trajectory along the Z-axis direction, the forward-backward motion trajectory along the X-axis direction, the pitch angle motion trajectory rotating around the Y-axis, and the motion trajectory of keeping the initial state in the other three degrees of freedom of the torso motion subtask; and generating the trajectory of keeping stable contact with the ground of the double-leg keeping static subtask.

[0060] It should be noted that after the whole body motion task is decomposed into multiple subtasks, further, the embodiment of the present application can generate the task space reference motion trajectory of each subtask in the future,

[0061] For the periodic squatting motion of the biped robot, the motion trajectory required by the subtask of the torso is as follows:

[0062] The up-down motion trajectory along the Z-axis direction;

[0063] The forward-backward motion trajectory along the X-axis direction;

[0064] The pitch angle motion trajectory rotating around the Y-axis;

[0065] The motion trajectory of keeping the initial state in the other three degrees of freedom;

[0066] The subtask of the double legs only needs the trajectory of keeping stable contact with the ground.

[0067] As a kind of can be realized way, in the periodic squatting motion of the biped robot, the reference trajectory of the embodiment of the present application can also be generated by a relatively simple method, that is, according to the instruction of the technical personnel, the reference position and speed at different time are directly specified, and the planning result is a triangular wave (specified speed) or square wave (specified position).

[0068] It should be appreciated by those skilled in the art that even if the technician specifies the amplitude, frequency, etc. of the squat, there are still countless choices for the reference motion trajectory of the robot, for example, a cosine curve, a triangular wave curve or even a square wave curve can be used as the reference motion trajectory in the Z-axis direction of the robot trunk, and even if the position and velocity are discontinuous, the control algorithm of the embodiment of the application can still achieve good tracking effect.

[0069] For other forms of motion, such as walking and somersault of a biped robot, in the specific implementation process, the control algorithm of the embodiment of the application can not make any requirements for the selection of the planning algorithm, and because the motion form of the robot is different, the planning algorithm required is also different, for example, in the walking motion process, the selection of the landing point of the leg swing is critical, and in the somersault motion process, the planning of the angular momentum of the center of mass is critical, and the planning algorithms required for the above two motions are usually completely different. For complex actions (high-difficulty actions) such as somersault, the planning algorithm and the control algorithm are also critical, so when the planning algorithm is not suitable, the control algorithm can not guarantee the good execution of the complex action.

[0070] It should be noted that in the specific implementation process, the technician can generate a reference trajectory according to the actual situation by using a suitable planning algorithm, which is not limited herein.

[0071] Therefore, the embodiment of the application generates a task space reference motion trajectory (including position and velocity) for each sub-task in the future period of time, thereby providing theoretical and technical support for realizing robot full-body control by using the RKP algorithm.

[0072] In step S102, the current state of the legged robot is obtained, and each component of the task space reference motion trajectory of each sub-motion task is calculated by using a preset real-time kinematics prediction RKP algorithm according to the current state, to obtain a plurality of expected accelerations.

[0073] After generating the task space reference motion trajectory of the plurality of sub-motion tasks in the future period of time, the embodiment of the application can use the RKP algorithm for each component of the reference motion trajectory of each sub-task according to the feedback state of the robot; wherein the tasks of the two legs and the trunk are 18 dimensions in total, and therefore 18 RKP optimization problems need to be solved to obtain the corresponding expected accelerations.

[0074] It should be noted that the RKP not only plays a role in online trajectory re-optimization in the above process, but also plays a role in feedback control.

[0075] Optionally, in an embodiment of the application, the expression of the optimization problem of the preset RKP algorithm is:

[0076]

[0077]

[0078] where, are the task space coordinates, velocity and acceleration of the k-th frame, respectively; is the reference state trajectory of the future N frames; N is the prediction horizon; δt is the time interval of the frame; is the decision variable vector, which is composed of the task space acceleration in the whole prediction horizon; is a positive definite diagonal matrix, which represents the weighted trajectory tracking error, is a positive number, which is the norm of the weighted system input.

[0079] It should be noted that the RKP algorithm adopted in the embodiments of the present application optimizes the position and velocity trajectory in the robot task space in a manner similar to MPC. However, unlike the conventional MPC, the RKP does not involve the dynamics model of the robot (whether it is a complete model or a simplified model), but considers the second-order linear kinematics of the robot task space motion, selects the task space acceleration as the system input, and replaces the inequality constraints in the cost function with penalties to regulate excessive system input.

[0080] Thus, the closed-form solution of the optimization problem can be derived, and the required computational cost is minimized.

[0081] In the embodiments of the present application, the expression of the RKP optimization problem is as follows:

[0082]

[0083] where, are the task space coordinates, velocity and acceleration of the k-th frame, respectively; is the reference state trajectory of the future N frames; N is the prediction horizon (i.e. the number of frames); δt is the time interval of the frame; is the decision variable vector, which is composed of the task space acceleration in the whole prediction horizon; is a positive definite diagonal matrix, which represents the weighted trajectory tracking error, is a positive number, which is the norm of the weighted system input.

[0084] It can be understood that, since the embodiments of the present application only use linear kinematics method to decouple different axial motions in the task space, the embodiments of the present application can be applied to any motion component in the task space through RKP.

[0085] Further, if RKP is regarded as a subsystem, its input is the current state and the future N-frame reference state trajectory, and its output is the N-frame optimal acceleration trajectory, thus, for the subsequent WBC, only the current frame optimal acceleration is needed

[0086] Let The state equation can be written as

[0087] X = A qp x0 + B qp U (2)

[0088] Wherein:

[0089]

[0090] And x0 is the state feedback, the cost function of formula (1) can be rewritten as the following formula:

[0091] J(U) = ||X ref -X|| Q +‖U‖ R (3)

[0092] Wherein:

[0093]

[0094]

[0095]

[0096] Substitute formula (2) into formula (3), an equivalent unconstrained QP problem can be obtained:

[0097]

[0098] Wherein:

[0099]

[0100]

[0101] Let The optimal solution of formula (1) can be obtained as follows:

[0102]

[0103] Wherein, Since Γ is independent of the state, it can be calculated in advance, which greatly reduces the time to solve problem formula (1); in addition, only needs to be applied Therefore, the embodiments of the application can select to calculate only the first row of U * .

[0104] Generally, the time complexity of solving equation (1) is O(N 2 ), but the time complexity of obtaining from equation (1) is O(N); generally, N≤100, so equation (1) can be solved in real time, thereby allowing RKP and subsequent WBC to be executed at the same frequency, while the time complexity of solving the optimization problem for most MPC methods cannot reach O(N).

[0105] It can be understood that, since RKP can consider the reference trajectory in the future period of time, and the state equation is simple and does not contain inequality constraints, RKP can run at a higher frequency; for example, in the control of squatting in place of a biped robot, the central processing unit of the Intel Core i7-11800H with a main frequency of 2.30GHz can be used in the embodiment of the application, and RKP can run at the same frequency as WBC at a frequency of 1kHz; in addition, a higher control frequency will obtain better control performance, for example, in the Mini-Cheetah quadruped robot of MIT, the MPC can run at 40Hz, and the WBC runs at 500Hz, for the WBC, the trajectory provided by the MPC is like a certain square wave, which can only occur a numerical jump every 25ms. In the embodiment of the application, since RKP and WBC run at the same frequency, for WBC, the trajectory provided by RKP is continuous and smooth, and when a certain disturbance occurs, RKP running at a higher frequency can respond faster than MPC running at a lower frequency.

[0106] In step S103, the plurality of desired accelerations are transmitted to the whole-body control WBC to coordinate the plurality of sub-motion tasks until the legged robot ends the whole-body motion task.

[0107] After obtaining the plurality of desired accelerations by using the RKP algorithm on each component of the reference motion trajectory of each sub-task respectively, further, the embodiment of the application can transmit the desired accelerations calculated by the plurality of RKP (corresponding to each degree of freedom of each sub-task) to the WBC, and use the WBC to coordinate the above-mentioned motion sub-tasks.

[0108] Optionally, in an embodiment of the application, transmitting the plurality of desired accelerations to the whole-body control WBC to coordinate the plurality of sub-motion tasks comprises: solving the optimal solution acceleration and the foot force spinor; based on the optimal solution acceleration and the foot force spinor, calculating the corresponding control instruction according to the type of the robot actuator, and controlling the robot actuator to execute the control instruction.

[0109] It can be understood that, since the RKP has played the role of feedback control, the WBC can be selected to be implemented by a single QP, and the optimization variable thereof can be selected to be acceleration in the generalized joint space and the foot force twist w f .

[0110] In addition, in addition to the torso task and the double-foot task mentioned above, embodiments of the present application can additionally add a task of minimizing the foot force twist (which does not require a reference trajectory), while considering appropriate constraints as described below:

[0111] a floating-base dynamics equation constraint;

[0112] a friction cone inequality constraint of the foot force;

[0113] a joint torque saturation constraint;

[0114] a joint angular velocity saturation constraint;

[0115] a joint power saturation constraint.

[0116] After the optimal solution and are solved, embodiments of the present application can calculate corresponding control instructions according to the type of the robot effector, for example, for a position-controlled effector, the joint position instruction can be directly obtained by twice integration of ; for a force-controlled effector, the joint torque instruction τ j can be calculated according to equation (6):

[0117]

[0118] where S j is a selection matrix of the driving joint, M is a generalized mass matrix, h is a term containing Coriolis force, centrifugal force and gravity, J f is a contact Jacobian matrix of the two feet.

[0119] Finally, the effector executes the joint position instruction or the joint torque instruction, and the biped robot can complete the squatting motion.

[0120] It should be noted that the WBC in the embodiments of the present application must be implemented by solving an optimization problem, but the specific implementation content is not limited, that is, it can be implemented by using WQP or HQP; in addition, since the skilled person can select appropriate optimization variables according to the actual situation, the physical meaning of the optimization variables of the optimization problem in the WBC is not specified in the embodiments of the present application; similarly, since the tasks and constraint settings of the WBC are different for different legged robots and different working conditions, the WBC in the embodiments of the present application does not limit the specific task or constraint setting, for example, in the process of squatting in place of the above-mentioned biped robot, not all of the constraint conditions of the WBC are necessary, and after removing a joint speed constraint or a joint power constraint, the control method is still feasible in the case of low-speed motion.

[0121] It can be understood that, since the calculation amount of the RKP is smaller than that of the WBC, the running frequency of the WBC can be further improved (such as from 500 Hz to 1 kHz) under the condition that the central processor computing power is unchanged; and when the control frequencies of the RKP and the WBC are already high (such as 1 kHz), the higher control frequency has a smaller effect on improving the control effect, the embodiments of the present application can limit the control frequencies of the RKP and the WBC (such as 1 kHz), so as to leave more computing power for the central processor to complete other tasks (such as the visual positioning calculation task of the robot), and further optimize and improve the overall control effect of the whole body motion of the robot.

[0122] According to the legged robot whole body control method combining real-time kinematics prediction provided in the embodiments of the present application, the complex robot motion equation and inequality constraint are processed by the WBC, the reference trajectory in the future period of time is considered by the RKP, and the motion trajectory is optimized again to obtain a more suitable motion trajectory, which is especially suitable for processing the reference trajectory with jumps, and the RKP has a closed-form solution, which can effectively improve the solving efficiency, improve the running frequency, greatly reduce the calculation amount, and make up for the defects of the WBC.

[0123] The legged robot whole body control method combining real-time kinematics prediction of the present application is described below in combination with the drawings.

[0124] Figure 2 The execution logic diagram of the legged robot whole body control method combining real-time kinematics prediction of the embodiments of the present application is shown in FIG. 1. Figure 2 As shown in FIG. 1, the execution steps of the legged robot whole body control method combining real-time kinematics prediction are as follows:

[0125] S201: According to the motion requirements of the user for the robot, the whole body motion task is divided into multiple sub-tasks, for example, the task of squatting in place of a biped robot can be divided into two sub-tasks of keeping the feet and the trunk still;

[0126] S202: generating, according to an instruction of a user, a task space reference motion trajectory (including position and velocity) of each subtask in a future period of time, for example, a reference motion trajectory on six degrees of freedom of a trunk;

[0127] S203: using an RKP algorithm on each component of the reference motion trajectory of each subtask according to a feedback state of the robot, to obtain a more appropriate and smooth acceleration trajectory;

[0128] S204: using a WBC to coordinate the acceleration trajectories of each subtask according to the feedback state of the robot, and finally outputting a control instruction, for a force control type driver, outputting a joint instance instruction; and for a position control type driver, outputting a joint position instruction.

[0129] Secondly, a full-body control device of a legged robot combining real-time kinematics prediction according to an embodiment of the present application is described with reference to the accompanying drawings.

[0130] Figure 3 is a block schematic diagram of the full-body control device of the legged robot combining real-time kinematics prediction according to an embodiment of the present application.

[0131] As shown in Figure 3 , the full-body control device of the legged robot combining real-time kinematics prediction 10 includes a generation module 101, a calculation module 102, and a coordination module 103.

[0132] The generation module 101 is configured to generate a task space reference motion trajectory of a full-body motion task of a legged robot in a future preset time length, which is decomposed into a plurality of sub motion tasks.

[0133] The calculation module 102 is configured to obtain a current state of the legged robot, and calculate each component of the task space reference motion trajectory of each sub motion task by using a preset real-time kinematics prediction RKP algorithm according to the current state, to obtain a plurality of expected accelerations.

[0134] The coordination module 103 is configured to transmit the plurality of expected accelerations to a full-body control WBC, to coordinate the plurality of sub motion tasks until the legged robot ends the full-body motion task.

[0135] Optionally, in an embodiment of the present application, an expression of an optimization problem of the preset RKP algorithm is as follows:

[0136]

[0137]

[0138] wherein, are a task space coordinate, a velocity, and an acceleration of the kth frame, respectively; is the reference state trajectory of future N frames; N is the prediction horizon; δt is the time interval of a frame; is the decision variable vector composed of the task space acceleration in the entire prediction horizon; is a positive definite diagonal matrix representing the weighted trajectory tracking error, is a positive number, which is the norm of the weighted system input.

[0139] Optionally, in an embodiment of the present application, the coordination module 103 comprises a solving unit and a control unit.

[0140] The solving unit is configured to solve the optimal solution of the acceleration and the foot force spinor.

[0141] The control unit is configured to calculate the corresponding control instruction according to the type of the robot actuator based on the optimal solution of the acceleration and the foot force spinor, and control the robot actuator to execute the control instruction.

[0142] Optionally, in an embodiment of the present application, the generation module 101 comprises a decomposition unit configured to decompose the in-place periodic squatting motion task of the legged robot into a double-foot-keeping-still-to-ground sub-motion task and a trunk motion sub-motion task.

[0143] Optionally, in an embodiment of the present application, the generation module 101 further comprises a first keeping unit and a second keeping unit.

[0144] The first keeping unit is configured to generate an up-and-down motion trajectory along the Z-axis direction, a forward-and-backward motion trajectory along the X-axis direction, a pitch angle motion trajectory rotating around the Y-axis, and motion trajectories keeping the initial state in the other three degrees of freedom of the trunk motion sub-motion task.

[0145] The second keeping unit is configured to generate a trajectory keeping stable contact with the ground of the double-foot-keeping-still-to-ground sub-motion task.

[0146] It should be noted that the foregoing explanation and description of the embodiment of the method for full-body control of a legged robot combined with real-time kinematic prediction also apply to the embodiment of the device for full-body control of a legged robot combined with real-time kinematic prediction, which will not be described here again.

[0147] The full-body control device of a legged robot combining real-time kinematic prediction provided by the embodiment of the application generates a task space reference motion trajectory of a full-body motion task of the legged robot decomposed into a plurality of sub-motion tasks within a future preset time length; acquires a current state of the legged robot, and respectively calculates each component of the task space reference motion trajectory of each sub-motion task by using a preset real-time kinematic prediction (RKP) algorithm according to the current state, to obtain a plurality of expected accelerations; and transmits the plurality of expected accelerations to the full-body control (WBC) to coordinate the plurality of sub-motion tasks until the legged robot ends the full-body motion task. The application processes complex robot motion equations and inequality constraints by using the WBC, considers the reference trajectory in the future for a period of time by using the RKP, and optimizes the reference trajectory again to obtain a more suitable motion trajectory, which is especially suitable for processing a reference trajectory with jumps. The RKP has a closed-form solution, which can effectively improve the solving efficiency, improve the operation frequency, greatly reduce the calculation amount, and make up for the defects of the WBC.

[0148] Figure 4 The structure schematic diagram of the electronic device provided by the embodiment of the application is provided. The electronic device can include:

[0149] The memory 401, the processor 402 and the computer program stored in the memory 401 and executable on the processor 402.

[0150] The processor 402 implements the full-body control method of the legged robot combining real-time kinematic prediction provided in the above embodiment when executing the program.

[0151] Further, the electronic device further includes:

[0152] The communication interface 404 is used for communication between the memory 401 and the processor 402.

[0153] The memory 401 is used to store the computer program executable on the processor 402.

[0154] The memory 401 can include a high-speed RAM memory, and can also include a non-volatile memory, for example, at least one disk memory.

[0155] If the memory 401, the processor 402 and the communication interface 404 are implemented independently, the communication interface 404, the memory 401 and the processor 402 can be connected with each other through a bus and complete communication between each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For convenience of representation, Figure 4 Only one thick line is used to represent the bus in the figure, but it does not mean that there is only one bus or only one type of bus.

[0156] Optionally, in a specific implementation, if the memory 401, the processor 402 and the communication interface 404 are integrated on a chip, the memory 401, the processor 402 and the communication interface 404 can complete communication between each other through an internal interface.

[0157] The processor 402 can be a Central Processing Unit (CPU), or an Application Specific Integrated Circuit (ASIC), or one or more integrated circuits configured to implement one or more embodiments of the present application.

[0158] The embodiment also provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to implement the above-mentioned full-body control method of a legged robot combined with real-time kinematic prediction.

[0159] In the description of the present specification, the description of the terms "one embodiment", "some embodiments", "an example", "a specific example" or "some examples" means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In the present specification, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or N embodiments or examples in a suitable manner. In addition, the person skilled in the art can combine and combine the different embodiments or examples described in the present specification and the features of the different embodiments or examples without contradiction.

[0160] Moreover, the terms "first", "second", "third", etc. are used herein only to describe different steps or claims and do not imply or imply a relative importance or a specific number of the indicated technical features. Thus, features defined with "first", "second" can explicitly or implicitly include at least one of the features. In the description of the present application, the meaning of "N" is at least two, for example, two, three, etc., unless otherwise explicitly specified.

[0161] Any process or method descriptions or descriptions of the flow diagrams described herein or otherwise described herein can be understood as representing the modules, segments or portions of code that include executable instructions for performing custom logic functions or processes, and the scope of preferred embodiments of the present application includes additional implementations that can not be expressly shown or described herein, including implementations that can be performed in a different order, including substantially simultaneously, or in reverse order, depending on the functionality involved, as would be understood by those skilled in the art of the embodiments described herein.

[0162] The logic and / or steps represented in the flow diagrams or otherwise described herein, for example, can be considered as a list of executable instructions for implementing the logic function, which can be specifically embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus or device, such as a computer-based system, a processor-based system, or other system that can fetch the instructions from the instruction execution system, apparatus or device and execute the instructions, or in conjunction with such instruction execution system, apparatus or device. For the purposes of this specification, a "computer-readable medium" can be any apparatus that can contain, store, communicate, propagate or transport the program for use by or in connection with the instruction execution system, apparatus or device, or in conjunction with such instruction execution system, apparatus or device. More specific examples (a non-exhaustive list) of computer-readable medium include the following: an electrical connection having one or more wires (electronic devices), a portable computer diskette (magnetic devices), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). In addition, the computer-readable medium can even be paper or other suitable medium on which the program can be printed, as the program can be electronically obtained from the paper or other suitable medium by optical scanning, then edited, interpreted or otherwise processed in electronic manner, if necessary, and then stored in the computer storage.

[0163] It should be understood that portions of the application can be realized with a combination of hardware, software, firmware, or their combination. In the above-described embodiments, the N steps or methods can be realized with software or firmware stored in a memory and executed by a suitable instruction execution system. As in another embodiment, if realized with hardware, any one or their combination of the following technologies known in the art can be used: discrete logic circuit with logic gate circuit for implementing logic functions on data signals, application specific integrated circuit with suitable combination logic gate circuit, programmable gate array (PGA), field programmable gate array (FPGA), etc.

[0164] Those skilled in the art of the present technology can understand that all or part of the steps carried out by the above-mentioned embodiment method can be completed by a program instructing the relevant hardware, and the program can be stored in a computer readable storage medium. When the program is executed, it includes one of the steps of the method embodiment or a combination thereof.

[0165] In addition, the functional units in each embodiment of the present application can be integrated into one processing module, or each unit can be physically present separately, or two or more units can be integrated into one module. The above-mentioned integrated module can be realized in the form of hardware or in the form of a software function module. The integrated module, if realized in the form of a software function module and sold or used as an independent product, can also be stored in a computer readable storage medium.

[0166] The above-mentioned storage medium can be a read-only memory, a magnetic disk or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it should be understood that the above-mentioned embodiments are exemplary and cannot be understood as limiting the present application, and those skilled in the art can make changes, modifications, replacements and variations to the above-mentioned embodiments within the scope of the present application.

Claims

1. A method for whole-body control of a legged robot combining real-time kinematic prediction, characterized in that, Includes the following steps: Generate a task space reference motion trajectory for a future preset time period by decomposing the whole-body motion task of a legged robot into multiple sub-motion tasks; The current state of the legged robot is obtained, and each component of the task space reference motion trajectory of each sub-motion task is calculated using a preset real-time kinematic prediction (RKP) algorithm based on the current state to obtain multiple desired accelerations. as well as The multiple desired accelerations are transmitted to the whole body control (WBC) to coordinate the multiple sub-motor tasks until the legged robot completes the whole body motion task. The optimization problem of the preset real-time kinematic prediction RKP algorithm is expressed as follows: ， ， ， in, The first The frame's task space coordinates, velocity, and acceleration; For the future The reference state trajectory of the frame; To predict the horizon; The time interval between frames; It is a decision variable vector, composed of task space accelerations throughout the entire prediction horizon; It is a positive definite diagonal matrix, representing the weighted trajectory tracking error. The norm of the input to the weighted system; Indicates the first k +1 frame's state vector; Represents the state matrix; Indicates the first k The state vector of a frame; Represents the control matrix; Indicates the first k Decision variables for a frame; Represents the vector of decision variables to be optimized. The objective function.

2. The method according to claim 1, characterized in that, The transmission of the multiple desired accelerations to the whole-body control (WBC) to coordinate the multiple sub-motor tasks includes: Find the optimal solution for acceleration and foot spin. Based on the acceleration and foot force rotation of the optimal solution, the corresponding control command is calculated according to the type of robot actuator, and the robot actuator is controlled to execute the control command.

3. The method according to claim 1, characterized in that, The generation of the task space reference motion trajectory, which is decomposed into multiple sub-motion tasks by the legged robot and takes place over a preset time period, includes: The stationary periodic squatting and standing motion task of the legged robot is decomposed into a sub-motion task of keeping both feet stationary with the ground and a sub-motion task of trunk movement.

4. The method according to claim 3, characterized in that, The generation of the task space reference motion trajectory for the legged robot, which is decomposed into multiple sub-motion tasks, over a future preset time period, also includes: Generate the vertical motion trajectory along the Z-axis, the forward and backward motion trajectory along the X-axis, the pitch angle motion trajectory around the Y-axis, and the motion trajectory that maintains the initial state on the other three degrees of freedom for the torso motion sub-motion task. Generate the trajectory for the sub-motion task of keeping both feet stationary on the ground to maintain stable contact with the ground.

5. A whole-body control device for a legged robot incorporating real-time kinematic prediction, characterized in that, include: The generation module is used to generate task space reference motion trajectories for a future preset time period, which are decomposed from the whole-body motion task of the legged robot into multiple sub-motion tasks. The calculation module is used to obtain the current state of the legged robot, and calculate each component of the task space reference motion trajectory of each sub-motion task according to the current state using the preset real-time kinematic prediction (RKP) algorithm to obtain multiple expected accelerations. as well as A coordination module is used to transmit the multiple desired accelerations to the whole body control (WBC) to coordinate the multiple sub-motor tasks until the legged robot finishes the whole body motion task. The optimization problem of the preset real-time kinematic prediction RKP algorithm is expressed as follows: ， ， ， in, The first The frame's task space coordinates, velocity, and acceleration; For the future The reference state trajectory of the frame; To predict the horizon; The time interval between frames; It is a decision variable vector, composed of task space accelerations throughout the entire prediction horizon; It is a positive definite diagonal matrix, representing the weighted trajectory tracking error. The norm of the input to the weighted system; Indicates the first k +1 frame's state vector; Represents the state matrix; Indicates the first k The state vector of a frame; Represents the control matrix; Indicates the first k Decision variables for a frame; Represents the vector of decision variables to be optimized. The objective function.

6. The apparatus according to claim 5, characterized in that, The coordination module includes: The solver unit is used to solve for the acceleration and foot spin of the optimal solution; The control unit is used to calculate the corresponding control commands based on the acceleration and foot force rotation of the optimal solution, according to the type of robot actuator, and to control the robot actuator to execute the control commands.

7. The apparatus according to claim 5, characterized in that, The generation module includes: The decomposition unit is used to decompose the stationary periodic squatting and standing motion task of the legged robot into a sub-motion task of keeping both feet stationary with the ground and a sub-motion task of trunk movement.

8. The apparatus according to claim 7, characterized in that, The generation module further includes: The first holding unit is used to generate the up-and-down motion trajectory along the Z-axis, the forward-and-backward motion trajectory along the X-axis, the pitch angle motion trajectory around the Y-axis, and the motion trajectory that maintains the initial state in the other three degrees of freedom of the torso motion sub-motion task. The second holding unit is used to generate the trajectory for the two feet to maintain stable contact with the ground during the stationary sub-motion task.

9. An electronic device, characterized in that, include: The memory, the processor, and the computer program stored in the memory and executable on the processor, the processor executing the program to implement the whole-body control method for a legged robot incorporating real-time kinematic prediction as described in any one of claims 1-4.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to implement the whole-body control method for a legged robot that incorporates real-time kinematic prediction as described in any one of claims 1-4.

Images