Biped robot motion control method based on human body motion collaborative analysis

Abstract

The invention relates to a biped robot motion control method based on human body motion collaborative analysis, belongs to the technical field of robot control, and solves the problem that the motion personification degree of an existing robot is low. Comprising the following steps: pre-collecting and processing human body motion data in different motion scenes, calculating motion primitives and weight coefficients in each motion scene, and putting the motion primitives and the weight coefficients into a primitive database; the state data of the biped robot are collected in real time, the current motion scene of the biped robot is recognized, the expected mass center track and the expected foot end motion track of the biped robot are calculated according to the current motion scene and the primitive database, then a whole-body control algorithm is adopted to execute a plurality of control subtasks, and the optimal joint torque is solved. And the optimal joint torque is converted into a control signal of an actuator, and movement of the biped robot is controlled and tracked. And the movement personification degree of the robot is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and particularly relates to a biped robot motion control method based on human motion coordination analysis. BACKGROUND

[0002] Biped robots need to maintain balance and stability in complex environments while achieving flexible motion and operation, which puts high requirements on motion control. Therefore, how to optimize the control method of biped robots and improve the motion anthropomorphism of biped robots has always been a hot and difficult research topic.

[0003] The prior art usually triggers lower limb joint motion by using foot end contact signals to control the stable walking of the robot; and achieves motion control of the robot by taking a predefined parameterized motion trajectory as a desired trajectory module.

[0004] However, the existing motion control strategy for biped robots fails to fully integrate the gait characteristics of human motion, resulting in significant differences between the generated gait and natural human gait. At the same time, the existing method lacks analysis of the multi-joint coordination motion mechanism of the human body in different motion scenarios, and the robot motion trajectory calculation is complex, the control real-time performance is poor, and it is not suitable for motion in complex environments. SUMMARY

[0005] In view of the above analysis, the embodiments of the present application aim to provide a biped robot motion control method based on human motion coordination analysis, to solve the problem of low motion anthropomorphism of existing biped robots.

[0006] The embodiments of the present application provide a biped robot motion control method based on human motion coordination analysis, comprising the following steps:

[0007] Pre-acquire and process human motion data in different motion scenarios, calculate motion primitives and weight coefficients in each motion scenario, and put them into a primitive database;

[0008] Real-time acquisition of the state data of the biped robot, identification of the current motion scenario of the biped robot, calculation of the desired center of mass trajectory and the desired foot end motion trajectory of the biped robot according to the current motion scenario and the primitive database; according to the desired center of mass trajectory and the desired foot end motion trajectory, a full-body control algorithm is used to execute multiple control sub-tasks to solve the optimal joint torque; the optimal joint torque is converted into an actuator control signal to control and track the motion of the biped robot.

[0009] Based on the further improvement of the above method, the pre-acquisition and processing of human motion data in different motion scenarios, the calculation of motion primitives and weight coefficients in each motion scenario, include:

[0010] According to the human motion data in different motion scenarios, a plurality of joint angles and center of mass positions of each sampling point in each motion scenario are obtained to construct a corresponding original motion matrix;

[0011] For the original motion matrix in each motion scenario, a left singular matrix, a singular value matrix and a right singular matrix are obtained through singular value decomposition;

[0012] According to the singular value matrix and the cumulative variance contribution rate, the number of principal components is obtained;

[0013] According to the number of principal components, the weight coefficient is obtained from the left singular matrix, and the motion primitive is obtained from the right singular matrix.

[0014] Based on the further improvement of the above method, the desired center of mass trajectory and the desired foot end motion trajectory of the biped robot are calculated according to the current motion scenario and the primitive database, including:

[0015] According to the current motion scenario, the corresponding motion primitive and weight coefficient are obtained from the primitive database to reconstruct the matrix to obtain the current reconstructed first motion matrix;

[0016] According to the ratio of the step frequency of the biped robot to the human step frequency, the time scale of the current reconstructed first motion matrix is adjusted, and according to the ratio of the leg length of the biped robot to the human leg length, the space scale of the current reconstructed first motion matrix is scaled to obtain the current desired motion matrix; the trajectory of the center of mass is taken out from the current desired motion matrix as the desired center of mass trajectory;

[0017] According to the trajectory of the center of mass and the joint angle in the current desired motion matrix, the foot end trajectory is calculated as the desired foot end motion trajectory.

[0018] Based on the further improvement of the above method, the current desired motion matrix is calculated by the following formula:

[0019]

[0020] Wherein, X r '(t) represents the current desired motion matrix of the biped robot, X r (t) represents the current reconstructed first motion matrix, a represents the space scale scaling coefficient, l robot represents the leg length of the biped robot, l human represents the leg length of the human body; b represents the time scale scaling coefficient, f robot represents the step frequency of the biped robot, f human represents the human step frequency; X r (b·t) represents the compression or stretching of the time axis of the current reconstructed first motion matrix.

[0021] Based on the further improvement of the above method, the expected foot trajectory is calculated by the following formula:

[0022] z foot = z CoM -h bias -l Thigh ·cosα-l Shank ·cosβ,

[0023] x foot = x CoM +l Thigh ·sinα+l Shank ·sinβ,

[0024] wherein x foot and z foot represent the position coordinates of the foot in the forward direction and the vertical direction respectively, x CoM and z CoM represent the position coordinates of the center of mass in the forward direction and the vertical direction respectively, h bias represents the height difference between the center of mass and the hip joint, l Thigh and l Shank represent the length of the thigh and the shank respectively, and α and β represent the hip joint angle and the knee joint angle respectively.

[0025] Based on the further improvement of the above method, the plurality of control sub-tasks have different priorities, and the priorities from high to low are: closed-loop dynamics model, support leg foot end contact constraint, body expected attitude tracking, body expected position tracking and swing leg foot trajectory tracking.

[0026] Based on the further improvement of the above method, the closed-loop dynamics model is represented by the following formula:

[0027]

[0028] wherein M(q) represents the joint space inertia matrix, h(q, u) represents the Coriolis force term, g(q) represents the gravity term, F L , F S and F C represent the closed-loop constraint force, the support leg foot end contact force and the body constraint force of the biped robot respectively, J L , J S and J C represent the Jacobian matrix of the closed-loop constraint force, the Jacobian matrix of the support leg foot end contact force and the Jacobian matrix of the body constraint force respectively, S represents the drive joint selection matrix, which is used to map the actuator torque to the joint space; and T represents the transpose operation of the matrix.

[0029] Based on the further improvement of the above method, the body desired position tracking is used for optimizing and tracking the body position by calculating the center of mass error between the desired center of mass trajectory and the actual center of mass trajectory in real time, and feedback controlling the center of mass error by using a proportional differential control algorithm.

[0030] Based on the further improvement of the above method, the swing leg foot end trajectory tracking is used for optimizing and tracking the foot end position by calculating the foot end error between the desired foot end motion trajectory and the actual foot end motion trajectory in real time, and feedback controlling the foot end error by using a proportional differential control algorithm.

[0031] Based on the further improvement of the above method, the motion scene is composed of a speed level, a terrain condition and a motion mode, the speed level includes slow, medium and fast, the terrain condition includes flat ground, uphill and downhill, and the motion mode includes walking and running.

[0032] Compared with the prior art, the present application can at least realize one of the following beneficial effects:

[0033] 1. By analyzing the multi-joint coordination characteristics of the human body, a primitive database under different motion scenes is established, the primitive database is called according to the real-time motion state of the robot, the desired center of mass trajectory and the desired foot end motion trajectory are generated, and the real-time application is applied to the robot whole body control algorithm, which improves the motion personification degree and complex terrain adaptability of the robot.

[0034] 2. By mining the coordination mechanism of the human body under multiple speed levels, multiple terrain conditions and multiple motion modes, motion primitives and weight coefficients are extracted, which greatly reduces the dimension of data, effectively reduces the calculation complexity of the traditional motion trajectory tracking method, and enhances the control real-time performance of the robot in complex terrain.

[0035] 3. According to the actual motion scene, the motion primitives and weight coefficients are gait synthesized, and the space and time scales are scaled according to the mapping relationship between the human body and the robot motion characteristics, so as to quickly generate the desired motion trajectory of the robot, and facilitate the robot to reproduce the human motion characteristics.

[0036] In the present application, the above technical solutions can also be combined with each other to realize more preferred combination schemes. Other features and advantages of the present application will be described in the subsequent specification, and some advantages will become apparent from the specification, or will be understood by implementing the present application. The purpose and other advantages of the present application can be realized and obtained from the contents specifically pointed out in the specification and the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0037] The accompanying drawings are included to provide a further understanding of the embodiments, and are incorporated in and constitute a part of this specification, illustrate embodiments of the application, and together with the description serve to explain the principles of the application, and should not be considered limiting of the present application in any way, in the accompanying drawings, like reference numerals refer to the same components throughout the drawings;

[0038] Figure 1 Figure 1 is a schematic diagram of a parallel four-bar linkage biped robot in an embodiment of the present application;

[0039] Figure 2 Figure 2 is a flow chart of a biped robot motion control method based on human motion coordination analysis in an embodiment of the present application;

[0040] Figure 3 Figure 3 is a schematic diagram of a leg model in a human motion process in an embodiment of the present application;

[0041] Figure 4 Figure 4 is a schematic diagram of a whole body control process of a biped robot in an embodiment of the present application. DETAILED DESCRIPTION

[0042] The preferred embodiments of the present application will be described in detail below with reference to the drawings, wherein the drawings constitute a part of this application and are used to explain the principles of the embodiments of the present application, but are not used to limit the scope of the present application.

[0043] In the design of a biped robot, the structure of the robot is often different according to different motion task requirements. Figure 1 Figure 1 is a schematic diagram of a parallel four-bar linkage biped robot, and the parallel four-bar linkage is a representative leg structure that has shown significant advantages in biped robots.

[0044] Firstly, the closed-loop four-bar linkage structure has high mechanical rigidity and high overall stability, which helps to improve the reliability of motion. Secondly, the structure converts the active control function of the ankle joint into the geometric constraint of the four-bar linkage closed-loop mechanism, and the motion analysis is performed in the sagittal plane, i.e., only the motion in the forward direction (X-axis) and the vertical ground direction (Z-axis) is considered, and the lateral (Y-axis) displacement and rotation are ignored. In this way, not only the system complexity is reduced, but also the key dynamic processes such as the change of the center of mass position, the change of the foot position, and the switching of the support phase and the swing phase are effectively preserved. Finally, the hip and knee joints in the structure are driven cooperatively through mechanical linkages, which can enhance the cooperative motion performance between the joints and improve the driving efficiency.

[0045] One specific embodiment of the present application is to control the motion of a biped robot with a parallel four-bar linkage leg structure, and discloses a biped robot motion control method based on human motion coordination analysis, as shown in Figure 2, which comprises the following steps: Figure 2

[0046] S1, human motion data under different motion scenarios is collected and processed in advance, motion primitives and weight coefficients under each motion scenario are calculated, and are put into a primitive database.

[0047] ​It should be noted that the acquisition device includes a motion capture system, a pressure treadmill and a host computer. The pressure treadmill sets multiple speed levels to simulate multiple terrain conditions, 39 marker points are placed on the body and joints of the subject, and multiple modes of motion are performed on the pressure treadmill to form different motion scenarios. Among them, the speed level includes: slow, medium and fast, such as: speed in (0, 0.5 m / s] is slow, (0.5 m / s, 1.0 m / s] is medium 1.0 m / s, (1.0 m / s, 1.5 m / s) is fast, the terrain condition includes: flat, uphill and downhill; the motion mode includes: walking and running.

[0048] That is, the motion scenario is composed of the speed level, the terrain condition and the motion mode. In this embodiment, there are 3 speed levels, 3 terrain conditions and 2 motion modes, which form 18 motion scenarios.

[0049] When the subject moves in each motion scenario, the Vicon motion capture system and the pressure treadmill synchronously acquire kinematic data and foot end pressure data, wherein the kinematic data includes the position of each marker point, the center of mass position and the joint angle of each sampling point; the foot end pressure data includes the ground reaction force of each sampling point.

[0050] Further, the motion primitives and weight coefficients in each motion scenario are calculated, including:

[0051] ①According to the human motion data in different motion scenarios, the joint angles and center of mass positions of each sampling point in each motion scenario are obtained, and the corresponding original motion matrix is constructed.

[0052] It should be noted that the human motion data collected in each motion scenario is processed by using ViconNexus and MATLAB software, including:

[0053] The missing values are filled by interpolation method; the time when the foot end contacts the ground and the time when the foot end leaves the ground are obtained according to the ground reaction force at each time, and then the gait cycle is divided; the kinematic data is filtered and normalized;

[0054] Finally, the joint angles and center of mass positions of each sampling point in each motion scenario are obtained, wherein the center of mass position is a three-dimensional coordinate, including the position coordinates of the center of mass in the forward direction, the vertical direction and the left-right direction. This embodiment focuses on the center of mass trajectory formed by the position of the center of mass in the forward direction and the vertical direction.

[0055] It should be noted that since the human body motion is symmetrical on both sides, for each motion scenario, according to the data of the center of mass in the vertical direction and the joint angles of one side at each sampling point, the corresponding original motion matrix X o, where M represents the number of input variables, which is the number of joints on one side plus the two position coordinates of the centroid; and N represents the number of sampling points.

[0056] For example, if the collected joint angles are the angles of the hip, knee and ankle joints of the lower limbs, plus the positions of the centroid in the forward direction and the vertical direction, a total of five groups of time series data are obtained, then M is 5, and N is the number of sampling points in the time series.

[0057] 2) For the original motion matrix in each motion scene, the left singular matrix, the singular value matrix and the right singular matrix are obtained through singular value decomposition.

[0058] The singular value decomposition of the original motion matrix reflects the human motion coordination mode through the spatial mode and the temporal mode.

[0059] For example, the singular value decomposition formula for a motion scene is as follows:

[0060] X o = UΣV (1),

[0061] where U represents the left singular matrix, with a dimension of MxM, which reflects the spatial mode in the motion data; V represents the right singular matrix, with a dimension of NxN, which reflects the temporal mode in the motion data; and Σ represents the singular value matrix, which is a diagonal matrix with a dimension of MxN, and the singular values are on the diagonal.

[0062] 3) The number of principal components is obtained according to the singular value matrix and the cumulative variance contribution rate.

[0063] The cumulative variance contribution rate is calculated according to the singular values in the singular value matrix through the following formula:

[0064]

[0065] where K represents the cumulative variance contribution rate, λ k represents the singular value on the kth row of the diagonal in the singular value matrix, represents the variance of the singular value on the kth row.

[0066] When the cumulative variance contribution rate calculated according to the first k singular values is greater than or equal to a preset threshold, such as 94%, the number of singular values extracted k is the number of principal components.

[0067] 4) The weight coefficients are obtained from the left singular matrix, and the motion primitives are obtained from the right singular matrix according to the number of principal components.

[0068] Specifically, the first k columns of the left singular matrix U are extracted to obtain the matrix as the weight coefficient W, i.e. W = U 1:M,1:k, which is used to describe the contribution of each input variable in the human motion synergy pattern; a matrix obtained by extracting the first k rows from the right singular matrix V is taken as the motion primitive H(t), that is, H(t) = V 1:k,1:N , which is used to describe the change of the motion primitive in the human motion synergy pattern at time t.

[0069] Similarly, according to the number k of principal components, a truncated singular value matrix Σ' is obtained by extracting the first k rows and the first k columns from the singular value matrix Σ, that is, Σ' = Σ 1:k,1:k .

[0070] The weight coefficient, the motion primitive and the truncated singular value matrix under each motion scenario are stored in the primitive database, so that the motion primitive and the weight coefficient under the same motion scenario can be called in real time according to the motion demand of the biped robot, and the motion adaptability of the robot in a complex environment is improved.

[0071] S2, real-time acquisition of state data of the biped robot, identification of the current motion scenario of the biped robot, calculation of the expected center of mass trajectory and the expected foot end motion trajectory of the biped robot according to the current motion scenario and the primitive database; according to the expected center of mass trajectory and the expected foot end motion trajectory, a full-body control algorithm is used to execute multiple control sub-tasks to solve the optimal joint torque; the optimal joint torque is converted into a control signal of an actuator to control and track the motion of the biped robot.

[0072] It should be noted that the real-time collected state data includes body position, left and right foot end position, pose, real-time speed, joint angle and angular velocity, etc. according to the inertial sensor and encoder installed on the biped robot.

[0073] Further, the current speed level is determined according to the real-time speed, the current terrain condition is determined according to the body position or pose of the biped robot at continuous multiple time points, and it is determined whether the current is walking or running according to the left and right foot end positions at continuous multiple time points, so as to identify the current motion scenario of the biped robot.

[0074] Further, the expected center of mass trajectory and the expected foot end motion trajectory of the biped robot are calculated according to the current motion scenario and the primitive database, including:

[0075] ①According to the current motion scenario, the corresponding motion primitive and weight coefficient are obtained from the primitive database to reconstruct the matrix to obtain the current first motion matrix.

[0076] It should be noted that the current reconstructed first motion matrix is obtained by the following formula:

[0077] X r (t) = W r Σ' r Hr (t) (3),

[0078] wherein, X r (t) represents the current reconstructed first motion matrix, W r ,∑' r and H r (t) respectively represent the weight coefficient corresponding to the current motion scene obtained from the primitive data, the truncated singular value matrix and the motion primitive.

[0079] It should be noted that the current reconstructed first motion matrix includes the reconstructed joint angle trajectory and the center of mass trajectory, wherein the center of mass trajectory includes the position of the forward direction and the position of the vertical direction of the center of mass at each sampling point.

[0080] ②According to the ratio of the step frequency of the biped robot to the human body step frequency, the time scale of the first motion matrix is adjusted, and according to the ratio of the leg length of the biped robot to the human body leg length, the spatial scale of the current reconstructed first motion matrix is scaled to obtain the current expected motion matrix; the trajectory of the center of mass is taken out from the expected motion matrix as the expected center of mass trajectory.

[0081] In order to improve the motion personification degree of the biped robot, according to the difference between the biped robot and the human body in leg length and step frequency, the spatial scale and the time scale of the current reconstructed first motion matrix are adjusted to obtain the current expected motion matrix of the biped robot.

[0082] Specifically, according to the ratio of the step frequency of the biped robot to the human body step frequency, the time scale of the current reconstructed first motion matrix is adjusted, and according to the ratio of the leg length of the biped robot to the human body leg length, the spatial scale of the current reconstructed first motion matrix is scaled, and the formula is as follows:

[0083]

[0084] wherein, X r '(t) represents the current expected motion matrix of the biped robot, a represents the spatial scale coefficient, which is equivalent to compressing or stretching each trajectory curve in the current reconstructed first motion matrix by a certain proportion; l robot represents the leg length of the biped robot, l human represents the leg length of the human body; b represents the time scale coefficient, which is equivalent to compressing or stretching the time axis of the current reconstructed first motion matrix; f robot represents the step frequency of the biped robot, f human represents the human body step frequency.

[0085] It should be noted that when the time scale of the first motion matrix is adjusted, the resampling function of matlab, such as resample function, is used to scale or stretch the time axis (composed of each sampling point) according to the time scale scaling coefficient, to obtain the adjusted time axis, and to generate the data corresponding to each time on the adjusted time axis; then the data corresponding to each time is multiplied by the spatial scale scaling coefficient to obtain the data after spatial scale scaling, which constitutes the current expected motion matrix.

[0086] The trajectory of the center of mass is taken out from the current expected motion matrix as the expected center of mass trajectory.

[0087] ③According to the trajectory of the center of mass in the current expected motion matrix and the joint angles, the foot trajectory is calculated as the expected foot end motion trajectory.

[0088] It should be noted that the leg model schematic diagram in the process of human motion is as shown in Figure 3 Figure 3 The CoM(x CoM ,z CoM ) represents the position of the center of mass, including: the position coordinate x CoM of the center of mass in the forward direction and the position coordinate z CoM in the vertical direction.

[0089] The foot end position (the position of the marker point at the ankle joint of the human body) corresponding to each time in the expected foot end motion trajectory includes: the position coordinate x foot of the foot end in the forward direction and the position coordinate z foot in the vertical direction, which is calculated by the following formula:

[0090] z foot =z CoM -h bias -l Thigh ·cosα-l Shank ·cosβ (5),

[0091] x foot =x CoM +l Thigh ·sinα+l Shank ·sinβ (6),

[0092] Wherein, h bias represents the height difference between the center of mass and the hip joint, l Thigh and l Shank respectively represent the length of the thigh and the length of the calf, and α and β respectively represent the hip joint angle and the knee joint angle.

[0093] ​It can be understood that formula (5) and formula (6) are calculated according to the centroid position at a moment to obtain the foot end position corresponding to the moment, and finally the foot end positions corresponding to each moment form the foot end motion trajectory.

[0094] When controlling the biped robot, a whole-body control (WBC) algorithm is adopted to decompose the overall control task into multiple control subtasks and execute them according to the priority from high to low. In combination with the expected centroid trajectory and the expected foot end motion trajectory of the biped robot calculated in real time, the target function of the task space is continuously optimized, so as to coordinate the key motions of the biped robot and realize dynamic balance and trajectory tracking. Compared with the traditional control which relies on a predefined parameterized trajectory template, the calculation of the dynamic and real-time expected trajectory in the embodiment makes the robot motion more in line with the biomechanical characteristics, and enhances the real-time control and motion anthropomorphism of the robot.

[0095] It should be noted that the multiple control subtasks have different priorities, and the priorities from high to low are as follows: closed-loop dynamic model, supporting leg foot end contact constraint, body expected attitude tracking, body expected position tracking and swing leg foot end trajectory tracking.

[0096] Among them, the closed-loop dynamic model is used to ensure the stability and consistency of the dynamic behavior of the robot, to meet the overall motion control requirements; the supporting leg foot end contact constraint is used to ensure the stability of the contact between the supporting leg foot end and the ground, to prevent sliding or disengagement; the body expected attitude tracking is used to control the attitude of the robot body, to maintain the expected orientation and balance; the body expected position tracking is used to guide the robot body to reach the specified spatial position, to achieve the overall motion target; and the swing leg foot end trajectory tracking is used to control the swing leg foot end to move along the predetermined trajectory, to ensure the accuracy and coordination of the gait.

[0097] (1) Closed-loop dynamic model

[0098] Specifically, the closed-loop dynamic model of the embodiment is based on open-loop dynamic modeling, and a pair of additional closed-loop constraint forces are introduced at the hinge point on the leg structure of the parallel four-bar biped robot to construct, as shown in Figure 1 .

[0099] It should be noted that the local coordinate system with the geometric center of the biped robot as the origin is used as the base coordinate system, and the internal state of the biped robot is described by using the base coordinate system; in addition, the interaction between the biped robot and the environment is described by using the inertial coordinate system.

[0100] The generalized coordinates q, the velocity u, the acceleration and the driving torque τ of the biped robot in the base coordinate system are defined as follows:

[0101]

[0102] where the right subscript IB denotes a vector pointing from the inertial frame I to the body frame B, and the left subscript I denotes the component in the inertial frame I; I r IB 、 I v IB 、 I ω IB 、 I a IB and denote the relative body position, linear velocity, angular velocity, linear acceleration and angular acceleration vectors, respectively; R IB denotes the rotation matrix for mapping coordinates in the body frame B to the inertial frame I. and denote the ith joint angle, angular velocity and angular acceleration, respectively; n j denotes the total number of joints of the robot; τ1, τ2, τ3 and τ4 denote the 4 joint torques applied by the actuators, the number of which is adjusted according to the actual situation.

[0103] Further, the closed-loop dynamics model is constructed by the following formula:

[0104]

[0105] where M(q) denotes the joint space inertia matrix, h(q, u) denotes the Coriolis force term, g(q) denotes the gravity term, F L , F S and F C denote the closed-loop constraint force, the support leg foot contact force and the body constraint force of the biped robot, respectively; J L , J S and J C denote the Jacobian matrix of the closed-loop constraint force, the Jacobian matrix of the support leg foot contact force and the Jacobian matrix of the body constraint force, respectively; S denotes the driven joint selection matrix for mapping the actuator torque to the joint space; T denotes the transpose operation of the matrix.

[0106] (2) Support leg foot contact constraint

[0107] It should be noted that the support leg foot contact constraint aims to ensure the stable contact of the support leg with the ground, preventing the foot from sliding, lifting off or penetrating the ground. This constraint is realized through the foot zero acceleration condition of formula (9) and the physical limit of non-negative foot contact force of formula (10):

[0108]

[0109] F S,z ≥ 0 (10),

[0110] where, and denote the generalized velocity and generalized acceleration, respectively; denotes the rate of change of the Jacobian matrix of the support leg foot end contact force with respect to time; F S,z denotes the component of the support leg foot end contact force normal to the ground.

[0111] (3) Body desired attitude tracking

[0112] Body desired attitude tracking ensures that the body attitude of the robot can accurately track the desired value, thereby maintaining overall balance and motion stability. This control subtask is achieved through attitude error feedback control, as shown in the following formula:

[0113]

[0114] where, θ = [φ, ψ, γ] T denotes the actual body attitude angle, including body roll angle, pitch angle and yaw angle, denotes the actual body attitude angular velocity, θ des , and denote the desired attitude angle, desired attitude angular velocity and desired attitude angular acceleration, respectively, K p_C and K d_C denote the proportional and derivative gain matrices for adjusting the body attitude angle and attitude angular velocity error.

[0115] (4) Body desired position tracking

[0116] Body desired position tracking is a key task for ensuring that the body position accurately tracks the desired centroid trajectory. This control subtask is used to calculate the centroid error between the desired centroid trajectory and the actual centroid trajectory in real time, and use a proportional-derivative control algorithm to feedback control the centroid error, adjust the convergence of the centroid position and centroid velocity error, optimize and track the body position; The formula is as follows:

[0117]

[0118] where, J CoM denotes the centroid position Jacobian matrix, p CoM and denote the current centroid position and centroid velocity, respectively, p CoM,des , and denote the desired centroid position, desired centroid velocity and desired centroid acceleration in the desired centroid trajectory, respectively, K p_CoM and K d_CoM denote the proportional and derivative gain matrices for adjusting the centroid position and velocity error.

[0119] (5) Swing leg foot trajectory tracking

[0120] Swing leg foot trajectory tracking is a key task for ensuring the swing leg foot position accurately tracks the desired foot trajectory. This control subtask is used to adjust the convergence of foot position and foot velocity errors by calculating the foot error between the desired foot trajectory and the actual foot trajectory in real time, and using a proportional-differential control algorithm to feedback control the foot error, and optimize and track the foot position.

[0121]

[0122] where J swing represents the swing leg foot position Jacobian matrix, p foot and p foot,des represent the current foot position and foot velocity, respectively, p , and p represent the desired foot position, desired foot velocity and desired foot acceleration in the desired foot trajectory, respectively, K p_swing and K d_swing represent the proportional and differential gain matrices for adjusting the foot position and velocity errors, respectively.

[0123] Further, each control subtask is defined as a linear equality and / or inequality constraint as shown in equation (14), and is solved in order of priority from high to low by the Hierarchical Quadratic Programming (HQP) method:

[0124]

[0125] where x = τ T represents the variable to be optimized, i.e., the joint torque. w i and v i are slack variables for minimizing the task error.

[0126] In the first equality constraint of equation (14), A i is the Jacobian matrix in the i-th control subtask T i , which maps the variable to be optimized x to the task space, b i is the desired value of the i-th control subtask, such as the desired center of mass trajectory and the desired foot trajectory; w i represents that the equality constraint is allowed to have an error, and the goal is to minimize this error.

[0127] In the second inequality constraint of equation (14), D i is also a mapping matrix, which is different from A iD i defines the boundary of the feasible region, f i is the boundary value of the constraint, v i represents a slight violation of the constraint is allowed in the inequality constraint.

[0128] Each control subtask is used to optimize joint torque. By solving all subtasks in order of priority from high to low, it is ensured that low-priority tasks will not affect the implementation of high-priority tasks, and finally an optimal solution is obtained.

[0129] Specifically, the first control subtask is solved, and a quadratic programming (QP) problem is directly solved to obtain an initial solution x_result. For the i th (i ≥ 2) control subtask, the Jacobian matrix A 1:i-1 of all higher-priority control subtasks is stacked in rows to form a total constraint matrix, and a null space projection matrix is calculated; for example, the null space of the second control subtask is based on the Jacobian matrix A 1 of the first control subtask, the null space of the third control subtask 3 is based on the joint Jacobian matrix A 1:2 of the first control subtask and the second control subtask, and so on. By constructing a QP problem in the null space, the solution x_result is updated, so that the optimization of low-priority tasks will not affect the already satisfied high-priority tasks. After solving all control subtasks, the optimal joint torque is obtained.

[0130] The whole body control process of the biped robot using the method of the embodiment is as shown in Figure 4 . In Figure 4 , first, parameters are input, including control parameters and gait parameters, wherein the control parameters include but are not limited to: attitude control, body position control, swing leg foot end trajectory control proportional and differential gain matrix; the gait parameters include but are not limited to: gait type, gait cycle, expected speed, expected acceleration. The control parameters are transmitted to the WBC control module, and the gait parameters are transmitted to the gait scheduler.

[0131] The state data of the biped robot is collected by using sensors, including: body position, attitude, speed, joint angle, angular velocity, etc., and then the current motion scene of the biped robot is identified. The state estimator integrates the data collected by the sensors and the gait parameters sent by the gait scheduler, calculates and updates the complete state information of the biped robot, and transmits it to the gait scheduler; and the foot end position and speed are calculated by forward kinematics method, and the joint torque is calculated by inverse kinematics method, and are transmitted to the WBC control module, which is convenient for debugging, checking and auxiliary calculation.

[0132] The gait scheduler plans the timing according to the gait parameters and the complete state information sent by the state estimator, obtains gait phase timing information, and transmits the gait phase timing information to the WBC control module. Meanwhile, according to the current motion scenario, the motion primitives and weight coefficients are obtained from the primitive database, and the expected center of mass trajectory and the expected foot trajectory of the biped robot are calculated in real time and transmitted to the WBC control module.

[0133] The WBC control module executes multiple control sub-tasks according to the real-time expected center of mass trajectory and the expected foot trajectory in descending order of priority by using the whole-body control algorithm, and solves the optimal joint torque, which is converted into a control signal by the joint-level PD controller (a feedback control strategy composed of proportional P and differential D elements) to drive the actuator to execute, including: if the current center of mass trajectory is significantly different from the expected center of mass trajectory, adjusting the proportional and differential gains of the body position control; if the current foot trajectory is significantly different from the expected foot trajectory, adjusting the proportional and differential gains of the swing leg foot trajectory control; so as to accurately track the center of mass and foot trajectory while ensuring the stability of the body.

[0134] Compared with the prior art, the biped robot motion control method based on human motion coordination analysis provided by the embodiment analyzes the multi-joint coordination characteristics of the human body, establishes a primitive database under different motion scenarios, calls the primitive database according to the real-time motion state of the robot, generates an expected center of mass trajectory and an expected foot trajectory, and applies the expected center of mass trajectory and the expected foot trajectory to the whole-body control algorithm of the robot in real time, thereby improving the motion personification degree and complex terrain adaptability of the robot. By mining the coordination mechanism of the human body under multiple speed levels, multiple terrain conditions and multiple motion modes, motion primitives and weight coefficients are extracted, the dimensionality of the data is greatly reduced, the calculation complexity of the traditional motion trajectory tracking method is effectively reduced, and the control real-time performance of the robot under complex terrain is enhanced. According to the actual motion scenario, the motion primitives and the weight coefficients are gait synthesized, and the space and time scales are scaled according to the mapping relationship between the human body and the robot motion characteristics, so as to quickly generate the expected motion trajectory of the robot, and facilitate the robot to reproduce the motion characteristics of the human body.

[0135] Those skilled in the art can understand that all or part of the processes of the above-mentioned embodiments can be completed by a computer program instructing related hardware, and the program can be stored in a computer readable storage medium. The computer readable storage medium includes a magnetic disk, an optical disk, a read-only memory, a random access memory, etc.

[0136] The above description is only a preferred embodiment of the present application, but the protection scope of the present application is not limited thereto, and any changes or replacements within the technical scope disclosed by the present application can be easily thought of by those skilled in the art, which should be covered within the protection scope of the present application.

Claims

1. A method for motion control of a biped robot based on human motion coordination analysis, characterized in that, The method comprises the following steps: Pre-acquire and process human motion data under different motion scenarios, calculate motion primitives and weight coefficients under each motion scenario, and put them into a primitive database; Real-time acquisition of the state data of the biped robot, identification of the current motion scenario of the biped robot, calculation of the expected center-of-mass trajectory and the expected foot-end motion trajectory of the biped robot according to the current motion scenario and the primitive database, execution of multiple control sub-tasks by using a full-body control algorithm according to the expected center-of-mass trajectory and the expected foot-end motion trajectory, solution of the optimal joint torque, conversion of the optimal joint torque into a control signal of an actuator, and control and tracking of the motion of the biped robot.

2. The biped robot motion control method based on human motion synergy analysis according to claim 1, wherein, The pre-acquisition and processing of human motion data under different motion scenarios, and the calculation of motion primitives and weight coefficients under each motion scenario, comprise: According to human motion data under different motion scenarios, a plurality of joint angles and center-of-mass positions of each sampling point under each motion scenario are obtained to construct corresponding original motion matrices; For the original motion matrix under each motion scenario, a left singular matrix, a singular value matrix and a right singular matrix are obtained through singular value decomposition; According to the singular value matrix and the cumulative variance contribution rate, the number of principal components is obtained; According to the number of principal components, the weight coefficients are obtained from the left singular matrix, and the motion primitives are obtained from the right singular matrix.

3. The biped robot motion control method based on human motion synergy analysis according to claim 1 or 2, characterized in that, According to the current motion scenario and the primitive database, the expected center-of-mass trajectory and the expected foot-end motion trajectory of the biped robot are calculated, which comprises: According to the current motion scenario, the corresponding motion primitives and weight coefficients are obtained from the primitive database to reconstruct a matrix and obtain a current reconstructed first motion matrix; According to the ratio of the step frequency of the biped robot to the human step frequency, the time scale of the current reconstructed first motion matrix is adjusted, according to the ratio of the leg length of the biped robot to the human leg length, the space scale of the current reconstructed first motion matrix is scaled, and a current expected motion matrix is obtained; the trajectory of the center of mass is taken out from the current expected motion matrix as the expected center-of-mass trajectory; According to the trajectory of the center of mass and the joint angles in the current expected motion matrix, the foot-end trajectory is calculated as the expected foot-end motion trajectory.

4. The method according to claim 3, wherein, The current expected motion matrix is calculated by the following formula: where X r (t) denotes the desired motion matrix of the biped robot at the current time, X r (t) denotes the current reconstructed first motion matrix, a denotes a spatial scale coefficient, l robot denotes the leg length of the biped robot, l human denotes the human leg length; b denotes a time scale coefficient, f robot denotes the step frequency of the biped robot, f human denotes the human step frequency; X r (b · t) denotes compressing or stretching the time axis of the current reconstructed first motion matrix.

5. The method according to claim 3, wherein, The expected foot-end motion trajectory is calculated by the following formula: z foot = z CoM - h bias - l Thigh • cos a - l Shank • cos b, x foot = x CoM + l Thigh • sin α + l Shank • sin β, where x foot and z foot represent the position coordinates of the foot end in the advancing direction and the vertical direction, respectively, x CoM and z CoM represent the position coordinates of the center of mass in the advancing direction and the vertical direction, respectively, h bias represents the height difference between the center of mass and the hip joint, l Thigh and l Shank represent the thigh and calf lengths, respectively, and α and β represent the hip joint and knee joint angles, respectively.

6. The method of claim 1, wherein the method is based on a human motion synergy analysis. The plurality of control sub-tasks have different priorities, and the priorities from high to low are: closed-loop dynamic model, support leg foot-end contact constraint, body expected attitude tracking, body expected position tracking and swing leg foot-end trajectory tracking.

7. The method according to claim 6, wherein, The closed-loop dynamic model is represented by the following formula: where M(q) represents the joint space inertia matrix, h(q, u) represents the Coriolis force term, g(q) represents the gravity term, F L , F S , and F C represent the closed-loop constraint force, the support leg foot contact force, and the body constraint force of the biped robot, respectively, J L , J S , and J C represent the Jacobian matrix of the closed-loop constraint force, the Jacobian matrix of the support leg foot contact force, and the Jacobian matrix of the body constraint force, respectively, S represents the actuator torque to joint space mapping matrix, and T represents the transpose operation of a matrix.

8. The method according to claim 6, wherein, The body expected position tracking is used to calculate the center-of-mass error between the expected center-of-mass trajectory and the actual center-of-mass trajectory in real time, and to perform feedback control on the center-of-mass error by using a proportional-differential control algorithm, so as to optimize and track the body position.

9. The method according to claim 6, wherein, The swing leg foot-end trajectory tracking is used to calculate the foot-end error between the expected foot-end motion trajectory and the actual foot-end motion trajectory in real time, and to perform feedback control on the foot-end error by using a proportional-differential control algorithm, so as to optimize and track the foot-end position.

10. The biped robot motion control method based on human motion synergy analysis according to claim 1, wherein, The motion scene is combined by a speed level, a terrain condition and a motion mode, the speed level includes slow speed, medium speed and fast speed, the terrain condition includes flat ground, uphill and downhill, and the motion mode includes walking and running.

Images