CN101414190A - Method and system for controlling apery robot stabilized walking based on effective stable domain - Google Patents

Abstract

The invention discloses a control method and system for stable walking of a humanoid robot based on an effective stable area, belonging to the field of automatic control. The method includes: measuring the ground reaction force of the robot through a force sensor; obtaining the resultant point of the ground reaction force of the robot according to the ground reaction force; judging whether the planned zero moment point and the resultant point of the ground reaction force are both effective and stable In the area; according to the judgment result, control the robot to walk. The device includes: a force sensor, a processing module, a judgment module and a control module. The system includes: a feedforward device, a real-time corrector and a servo driver. The invention corrects the ankle joint angle of the robot in real time, so that the planned ZMP point and the resultant force point of the ground reaction force are all in the effective stable area, thereby realizing the stable walking of the robot.

Description

Control method and system for stable walking of humanoid robot based on effective stable area

technical field

The invention relates to the field of automation control, in particular to a control method and system for stable walking of a humanoid robot based on an effective stable area.

Background technique

Humanoid robots (hereinafter referred to as robots) move by walking on two legs, just like people. Its legs are similar to human beings, and it has better maneuverability than traditional wheeled and tracked robots, especially on uneven ground, stairs, and occasions where there are only discrete and discontinuous contact points with the ground. Superiority. However, biped robots are inherently unstable and prone to falls. In order for the robot to walk, it is necessary to give the walking trajectory (dynamic gait) of the robot. The dynamic gait of a robot is an inherent, periodic motion that is generated according to the overall dynamics of a biped robot. Due to the coupling of constraints and the complexity of dynamic equations, dynamic gait calculation requires an optimization process. Therefore, dynamic gaits can generally only be achieved by offline computational methods. That is, dynamic gaits are generally generated under the assumption that the biped robot model and the surrounding environment are known. In fact, the real environment of a biped robot cannot be exactly the same as the set environment and conditions. Due to changes in the surrounding environment of the robot or unknown conditions, if the robot performs mechanically in accordance with the pre-planned dynamic gait, the planned Real-time correction and control of the dynamic gait of the human body may cause abnormal phenomena such as instability or even falling. Therefore, it is necessary to correct the planned dynamic gait according to the current environmental information and the current state of the robot, and perform real-time gait control to overcome environmental changes and uncertainties, so that the robot can walk stably in the actual environment.

Prior art 1 discloses a control method based on zero moment point (ZMP, Zero Moment Point) compensation, specifically by changing the upper body of the robot and correcting the position of the foot of the robot to realize ZMP compensation, so that the robot can walk stably;

Among them, the specific meaning of ZMP is as follows: According to the principles of mechanics, when an object is in a static state, the necessary and sufficient condition for its balance is that the projection of its center of gravity on the ground falls on its supporting surface; and when the object is in a moving state, its balance The necessary condition is that the extension line of the resultant force of gravity and inertial force passes through its support surface, and the intersection point of the extension line of the resultant force and the support surface is called ZMP.

Prior Art 2 discloses a method for controlling a bipedal walking robot. Specifically, according to the ground reaction force, the roll angle and pitch angle are calculated, and the brake is driven according to the roll angle and pitch angle, so that the normal vector of the force-bearing surface Align with the reference vector in the direction of gravity to realize stable walking of the robot.

In the process of realizing the present invention, the inventor finds that there are at least the following problems in the prior art:

1) The state correction of the robot in the prior art 1 requires dynamic calculations, and it is difficult to achieve real-time compensation and correction; in addition, this method does not consider the stability margin necessary for the dynamic walking of the robot.

2) The prior art two is only suitable for the static walking occasion of the robot, but not suitable for the dynamic walking control of the robot.

Contents of the invention

In order to make the robot walk stably, an embodiment of the present invention provides a control method and system for stably walking a humanoid robot based on an effective stable region. Described technical scheme is as follows:

On the one hand, an embodiment of the present invention provides a control method for stable walking of a humanoid robot based on an effective stable area, the method comprising:

The ground reaction force of the robot is measured by the force sensor;

According to the ground reaction force, the resultant point of the ground reaction force of the robot is obtained;

Judging whether the planned zero-moment point and the resultant point of the ground reaction force are both within the effective stable area;

According to the judgment result, the robot is controlled to walk.

On the other hand, an embodiment of the present invention provides a control device for stable walking of a humanoid robot based on an effective stable area, and the device includes:

The force sensor is used to measure the ground reaction force of the robot;

The processing module is used to obtain the resultant force point of the ground reaction force of the robot according to the ground reaction force measured by the force sensor;

A judging module, used to judge whether the planned zero-moment point and the resultant ground reaction force point obtained by the processing module are all within the effective stable area;

A control module, configured to control the robot to walk according to the judgment result of the judgment module.

On the other hand, the embodiment of the present invention also provides a control system for stable walking of a humanoid robot based on an effective stable area, and the system includes:

Feedforward, used to provide the dynamic gait θ _a0 (t) of the offline planning of the robot;

A real-time modifier, used to correct the dynamic gait θ _a0 (t) in the feedforward when the resultant ground reaction force point and/or the planned zero moment point of the robot are not in the effective stable region The real-time correction amount Δθ _a (t);

The servo driver is used to drive the ankle joint of the robot after adding the dynamic gait θ _a0 (t) in the feedforward unit and the real-time correction amount Δθ _a (t) in the real-time corrector.

The beneficial effects of the technical solution provided by the embodiments of the present invention are:

By correcting the ankle joint angle of the robot in real time, the planned ZMP point and the resultant force point of the ground reaction force are all in the effective stable area, realizing the stable walking of the robot; moreover, the calculation is simple and the correction is fast without the need for a clear robot dynamic model The dynamic gait of the robot enables the robot to adapt to the unknown environment.

Description of drawings

Fig. 1 is the structural representation of the walk controller of robot;

Fig. 2 is a schematic diagram of the relationship between the ZMP point of the robot and the resultant force point of the ground reaction force and the stable area;

3 is a flow chart of a control method for stable walking of a humanoid robot based on an effective stable area provided by Embodiment 1 of the present invention;

Fig. 4 is a schematic diagram of a control device for stable walking of a humanoid robot based on an effective stable area provided by Embodiment 2 of the present invention;

Fig. 5 is a schematic diagram of a control system for a stable walking of a humanoid robot based on an effective stable area provided by Embodiment 3 of the present invention.

Detailed ways

In order to make the object, technical solution and advantages of the present invention clearer, the implementation manner of the present invention will be further described in detail below in conjunction with the accompanying drawings.

Referring to Fig. 1, it is a structural schematic diagram of the walking controller of the robot. The walking controller consists of the dynamic gait θ _a0 (t) of the ankle joint (the ankle joint angle of the offline planning) which plays a feedforward role and the local feedback function The real-time correction amount Δθ _a (t) of the ankle joint is formed, and the robot includes an upper body 101, a hip joint 102, a knee joint 103, an ankle joint 104, a foot 105, a force sensor 106 and the connection between each part, and the force sensor 106 uses to measure the magnitude of the ground reaction force.

The robot is divided into a single-foot support period and a double-foot support period. When in the single-foot support period, the ground reaction force of the single foot is measured through the force sensor of the single foot; The ground reaction forces were measured separately for both feet. Through the ground reaction force measured by the force sensor, the resultant force point of the ground reaction force can be obtained, specifically: when in the single-foot support period, the point of the ground reaction force of the single foot measured by the force sensor of the single foot is the ground Reaction force resultant force point; when in the support period of both feet, the force point of the resultant force of the ground reaction force of both feet respectively measured by the force sensors of both feet is the resultant force point of ground reaction force.

During the walking process of the robot, in order to avoid the robot toppling over and ensure the stable walking of the robot, the planned ZMP point (the ZMP point planned in advance to ensure the stable walking of the robot) and the resultant force point of the ground reaction force must be in the stable area. If the planned ZMP point and the resultant point of the ground reaction force are in the stable area, generally, the robot can walk without falling over. However, in order to ensure that the robot can walk stably, a certain distance should be kept between the planned ZMP point and the resultant point of the ground reaction force and the boundary of the stable area, so that the planned ZMP point and the resultant point of the ground reaction force are within the effective stable area, that is, there is a certain stability margin. Referring to Fig. 2, the stable area boundary 201 of robot is shown, effective stable area 202 and planning ZMP point 203 and ground reaction force resultant force point 204, d _zmp is planning ZMP point 203 (when not in effective stable area) and effective stable area The distance of the boundary 205, d _f is the distance between the ground reaction force resultant point 204 (when not in the effective stable area) and the effective stable area boundary 205 .

In the embodiment of the present invention, when the robot walks stably, the servo control reference angle θ _a (t) of the robot ankle joint (the actual ankle joint angle when the robot walks) is equal to the dynamic gait θ _a0 (t) of offline planning and the real-time correction amount Δθ The sum of _a (t), that is, θ _a (t) = θ _a0 (t) + Δθ _a (t). Among them, θ _a0 (t) is generally generated when the robot model and the surrounding environment are known; Δθ _a (t) is when the robot’s ground reaction force resultant point and/or planning ZMP point is not in the effective stable area , the real-time correction amount to the ankle joint angle when the robot maintains balance.

The following will specifically describe how to use the real-time correction amount of the ankle joint angle to correct the ankle joint angle in real time to avoid the robot from toppling over and ensure the robot to walk stably.

Example 1

Referring to FIG. 3 , it is a flow chart of a control method for stable walking of a humanoid robot based on an effective stable area provided by an embodiment of the present invention, which is used for real-time correction of the ankle joint angle to ensure stable walking of the robot, specifically including:

301: The ground reaction force is measured by the force sensor, and a ground reaction force resultant point is obtained according to the ground reaction force measured by the force sensor.

When the robot is stationary or walking, it can measure the magnitude of the ground reaction force through the force sensor.

During the walking process of the robot, it can be divided into a single-leg support period and a double-leg support period. When in the single-leg support period, the ground reaction force of the single foot is measured by the force sensor of the single foot; when in the double-leg support period, The ground reaction force of both feet is measured separately by the force sensors of both feet. When the robot is at rest and when the robot is walking, the two-foot support period will not be repeated here.

Through the ground reaction force measured by the force sensor, the resultant force point of the ground reaction force can be obtained, specifically: when in the single-foot support period, the point of the ground reaction force of the single foot measured by the force sensor of the single foot is the ground Reaction force resultant force point; when in the support period of both feet, the force point of the resultant force of the ground reaction force of both feet respectively measured by the force sensors of both feet is the resultant force point of ground reaction force.

302: Determine whether the planned ZMP point and the resultant ground reaction force point are both within the effective stable area.

Judging whether the planned ZMP point and the resultant ground reaction force point are both in the effective stable area, the possible results are as follows:

Both the planned ZMP point and the resultant point of ground reaction force are within the effective stable area;

The planned ZMP point is not in the effective stable area, and the resultant ground reaction force point is in the effective stable area;

The planned ZMP point is in the effective stable area, and the resultant ground reaction force point is not in the effective stable area;

Neither the planned ZMP point nor the resultant point of ground reaction force is in the effective stable area.

303: Control the robot to walk according to the judgment result of step 302.

According to the judgment result of step 302, controlling the robot to walk specifically includes the following situations:

If the judgment result of step 302 is that both the planned ZMP point and the ground reaction resultant force point are within the effective stable area, the robot walks according to the dynamic gait planned offline. That is to say, there is no need to correct the ankle joint angle of the robot, and the ankle joint angle of the robot is the dynamic gait θ _a0 (t) planned offline.

If the judgment result of step 302 is that the planned ZMP point is not in the effective stable area, and the resultant point of the ground reaction force is in the effective stable area, then the ankle joint angle of the supporting foot is corrected according to the distance between the planned ZMP point and the boundary of the effective stable area, Adjust the planning ZMP point to the effective stable area.

The ankle joint angle of the supporting foot is corrected according to the distance between the planned ZMP point and the boundary of the effective stable area. The specific process is: on the basis of the offline planned dynamic gait θ _a0 (t), add the real-time correction amount Δθ _a (t ), to achieve real-time correction of the ankle joint angle. The dynamic gait θ _a0 (t) is planned offline and is a known quantity. The real-time correction Δθ _a (t) is the correction to the dynamic gait θ _a0 (t) calculated in real time during the actual walking process of the robot. quantity.

The real-time correction amount Δθ _a (t) is calculated according to the following formula (1):

${\mathrm{<span\; class="notranslate">\; \&Delta;\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; a</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; \&Integral;</span>}_{\mathrm{<span\; class="notranslate">\; 0</span>}}^{\mathrm{<span\; class="notranslate">\; t</span>}}\mathrm{<span\; class="notranslate">\; \&phi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; dt</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

In the formula, $\mathrm{\&phi;}\left(t\right)=\left\{\begin{array}{cc}{K}_{1c}*d_{\mathrm{zmp}}\left(t\right),& f_{\mathrm{foot}}>0\\ -K_{1r}*{\mathrm{\&Delta;\&theta;}}_a(t-T),& f_{\mathrm{foot}}=0\end{array}\right.,$ d _zmp (t) represents the distance between the planned ZMP point and the boundary of the effective stable area at time t, K _1c and K _1r are coefficients, and 0<K _1c <1, 0<K _1r <1, T is the computer control period, F _foot It is the magnitude of the ground reaction force on the foot measured by the force sensor. F _foot >0 means that the foot is in a supporting state, and F _foot = 0 means that the foot is in a swinging state, that is, when the foot in the supporting state is converted to a swinging state, according to F The formula corresponding to _foot = 0 corrects the ankle joint angle of the foot. When the foot is in a swing state, the ankle joint angle of the foot should gradually return to the value specified by the inherent dynamic gait, so the formula corresponding to F _foot = 0 is Return the ankle angle of that foot to the value dictated by the intrinsic dynamic gait.

It should be noted that the robot is divided into a single-foot support period and a double-foot support period. When in the single-foot support period, the ankle joint angle of the single foot is corrected according to the distance between the planned ZMP point and the boundary of the effective stable area, so that the planned Adjust the ZMP point to the effective stable area; when the feet are in the support period, the ankle joint angles of both feet are corrected according to the distance between the planned ZMP point and the boundary of the effective stable area, so that the planned ZMP point is adjusted to the effective stable area .

If the judgment result of step 302 is that the planned ZMP point is in the effective stable area, and the ground reaction force resultant point is not in the effective stable area, the ankle joint angle of the supporting foot is corrected according to the distance between the ground reaction force resultant point and the boundary of the effective stable area , so that the resultant force point of the ground reaction force is adjusted to the effective stable area.

The ankle joint angle of the supporting foot is corrected according to the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area. The specific process is: add the real-time correction amount Δθ _a to the dynamic gait θ _a0 (t) planned offline (t), realizing the correction of the ankle joint angle. The dynamic gait θ _a0 (t) is planned offline and is a known quantity. The real-time correction Δθ _a (t) is the correction to the dynamic gait θ _a0 (t) calculated in real time during the actual walking process of the robot. quantity.

The real-time correction amount Δθ _a (t) is calculated according to the following formula (2):

d_f(t)表示t时刻地面反作用力合力点与有效稳定区域边界的距离，K_2c和K_2r是系数，且0<K_2c<1、0<K_2r<1，T是计算机控制周期，F_foot是力传感器测得的对脚的地面反作用力的大小，F_foot>0表示脚处于支撑状态，F_foot＝0表示脚处于摆动状态，也就是处于支撑状态的脚转换到摆动状态时，按照F_foot＝0对应的公式对该脚的踝关节角度进行修正，当脚处于摆动状态时，该脚的踝关节角度应该逐渐恢复到固有动态步态规定的值，所以F_foot＝0对应的公式是使该脚的踝关节角度恢复到固有动态步态规定的值。In the formula,

d _f (t) represents the distance between the ground reaction force resultant point and the boundary of the effective stable area at time t, K _2c and K _2r are coefficients, and 0<K _2c <1, 0<K _2r <1, T is the computer control cycle, F _foot is the magnitude of the ground reaction force on the feet measured by the force sensor. F _foot >0 means that the feet are in a support state, and F _foot = 0 means that the feet are in a swing state, that is, when the feet in a support state switch to a swing state, Correct the ankle joint angle of the foot according to the formula corresponding to F _foot = 0. When the foot is in a swing state, the ankle joint angle of the foot should gradually return to the value specified by the inherent dynamic gait, so the corresponding value of F _foot = 0 The formula is to return the ankle angle of that foot to the value dictated by the inherent dynamic gait.

It should be noted that the robot is divided into a single-foot support period and a double-foot support period. When in the single-foot support period, the ankle joint angle of the single foot is corrected according to the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area. Adjust the resultant force point of the ground reaction force to the effective stable area; when in the support period of the feet, correct the ankle joint angles of both feet according to the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area, so that the ground reaction force The resultant force point is adjusted to the effective stable area.

If the judgment result of step 302 is that neither the planning ZMP point nor the ground reaction force resultant point is in the effective stable area, then for the ankle angle of the supporting foot, according to the distance between the planning ZMP point and the boundary of the effective stable area and the ground reaction force resultant point and The distance product of the boundary of the effective stable area is corrected, so that the planned ZMP point and the resultant force point of the ground reaction force are both adjusted to the effective stable area.

For the ankle joint angle of the supporting foot, it is corrected according to the product of the distance between the planned ZMP point and the boundary of the effective stable area and the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area. The specific process is: in the offline planned dynamic gait θ _a0 On the basis of (t), add the real-time correction amount Δθ _a (t) to realize the correction of the ankle joint angle. The dynamic gait θ _a0 (t) is planned offline and is a known quantity. The real-time correction Δθ _a (t) is the correction to the dynamic gait θ _a0 (t) calculated in real time during the actual walking process of the robot. quantity.

The real-time correction amount Δθ _a (t) is calculated according to the following formula (3):

${\mathrm{<span\; class="notranslate">\; \&Delta;\&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; a</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; \&Integral;</span>}_{\mathrm{<span\; class="notranslate">\; 0</span>}}^{\mathrm{<span\; class="notranslate">\; t</span>}}\mathrm{<span\; class="notranslate">\; \&psi;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; dt</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

In the formula, $\mathrm{\&psi;}\left(t\right)=\left\{\begin{array}{cc}{K}_{3c}*d_{\mathrm{zmp}}\left(t\right)*d_f\left(t\right),& f_{\mathrm{foot}}>0\\ -K_{3r}*{\mathrm{\&Delta;\&theta;}}_a(t-T),& f_{\mathrm{foot}}=0\end{array}\right.,$ d _zmp (t) represents the distance between the planned ZMP point and the boundary of the effective stable area at time t, d _f (t) represents the distance between the resultant point of ground reaction force and the boundary of the effective stable area at time t, K _3c and K _3r are coefficients, and 0 <K _3c <1, 0<K _3r <1, T is the computer control cycle, F _foot is the magnitude of the ground reaction force on the foot measured by the force sensor, F _foot >0 means the foot is in a supported state, F _foot ＝0 Indicates that the foot is in a swing state, that is, when the foot in the support state is converted to a swing state, the ankle joint angle of the foot is corrected according to the formula corresponding to F _foot = 0, and when the foot is in a swing state, the ankle joint angle of the foot is It should gradually return to the value specified by the inherent dynamic gait, so the formula corresponding to F _foot =0 is to restore the ankle joint angle of the foot to the value specified by the inherent dynamic gait.

It should be noted that the robot is divided into a single-foot support period and a double-foot support period. When in the single-foot support period, the angle of the ankle joint of the single foot is determined according to the distance between the planned ZMP point and the boundary of the effective stable area and the resultant force of the ground reaction force The product of the distance between the point and the boundary of the effective stable area is corrected, so that the planned ZMP point and the resultant force point of the ground reaction force are adjusted to the effective stable area; The product of the distance between the ZMP point and the boundary of the effective stable area and the distance between the resultant point of the ground reaction force and the boundary of the effective stable area is corrected so that the planned ZMP point and the resultant point of the ground reaction force are adjusted to the effective stable area.

In the method described in the embodiment of the present invention, by correcting the ankle joint angle of the robot in real time, the planned ZMP point and the resultant force point of the ground reaction force are all within the effective stable area, and the stable walking of the robot is realized; and no specific robot is required. The dynamic model is simple to calculate, and can quickly correct the dynamic gait of the robot, so that the robot can adapt to the unknown environment.

Example 2

Referring to Fig. 4, an embodiment of the present invention provides a control device for stable walking of a humanoid robot based on an effective stable area, the device specifically includes:

The force sensor 401 is used to measure the ground reaction force of the robot;

The processing module 402 is used to obtain the resultant force point of the ground reaction force of the robot according to the ground reaction force measured by the force sensor 401;

Judging module 403, for judging whether the planned zero-moment point and the resultant ground reaction force point obtained by the processing module 402 are all within the effective stable area;

The control module 404 is configured to control the robot to walk according to the determination result of the determination module 403 .

Wherein, the control module 404 specifically includes:

The processing unit is configured to make the robot walk according to the planned dynamic gait when the judging result of the judging module 403 is that the planned zero-moment point and the resultant ground reaction force point are both within the effective stable area.

Wherein, the control module 404 specifically includes:

The first correction unit is used to, when the judging result of the judging module 403 is that the planned zero-moment point is not in the effective stable area, and the resultant force point of the ground reaction force is in the effective stable area, adjust the angle of the ankle joint of the supporting foot according to the planned zero-moment point The distance to the boundary of the effective stable area is corrected. The specific correction method is: on the basis of the dynamic gait θ _a0 (t) planned offline, the real-time correction amount Δθ _a (t) is added to realize the adjustment of the ankle joint angle. Correction in real time. The dynamic gait θ _a0 (t) is planned offline and is a known quantity. The real-time correction Δθ _a (t) is the correction to the dynamic gait θ _a0 (t) calculated in real time during the actual walking process of the robot. The real-time correction amount Δθ _a (t) is calculated according to the formula (1) in Example 1.

Wherein, the control module 404 specifically includes:

The second correction unit is used to, when the judging result of the judging module 403 is that the planned zero-moment point is in the effective stable area, and the resultant force point of the ground reaction force is not in the effective stable area, the angle of the ankle joint of the supporting foot is calculated according to the resultant force of the ground reaction force The distance between the point and the boundary of the effective stable area is corrected. The specific correction method is: add the real-time correction amount Δθ _a (t) on the basis of the dynamic gait θ _a0 (t) planned offline to realize the adjustment of the ankle joint angle real-time corrections. The dynamic gait θ _a0 (t) is planned offline and is a known quantity. The real-time correction Δθ _a (t) is the correction to the dynamic gait θ _a0 (t) calculated in real time during the actual walking process of the robot. The real-time correction amount Δθ _a (t) is calculated according to the formula (2) in Example 1.

Wherein, the control module 404 specifically includes:

The third correction unit is used for when the judging result of the judging module 403 is that neither the planned zero-moment point nor the resultant point of the ground reaction force is within the effective stable area, for the ankle joint angle of the supporting foot, according to the planned zero-moment point and the effective stable area The distance between the boundary and the product of the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area is corrected. The specific correction method is: adding the real-time correction amount Δθ on the basis of the dynamic gait θ _a0 (t) planned offline _a (t), to achieve real-time correction of the ankle joint angle. The dynamic gait θ _a0 (t) is planned offline and is a known quantity. The real-time correction Δθ _a (t) is the correction to the dynamic gait θ _a0 (t) calculated in real time during the actual walking process of the robot. The real-time correction amount Δθ _a (t) is calculated according to the formula (3) in Example 1.

The device shown in the embodiment of the present invention, through the real-time correction of the ankle joint angle of the robot, makes the planned ZMP point and the resultant force point of the ground reaction force within the effective stable area, and realizes the stable walking of the robot; The dynamic model is simple to calculate, and can quickly correct the dynamic gait of the robot, so that the robot can adapt to the unknown environment.

Example 3

Referring to Fig. 5, an embodiment of the present invention provides a control system for stable walking of a humanoid robot based on an effective stable area. The system specifically includes:

Feedforwarder 501, for providing the dynamic gait θ _a0 (t) of the ankle joint of the offline planning of the robot;

The real-time modifier 502 is used to provide an ankle for correcting the dynamic gait θ _a0 (t) in the feedforwarder 501 when the resultant force point of the ground reaction force of the robot and/or the planned zero-moment point are not in the effective stable region Joint real-time correction Δθ _a (t);

When the planning ZMP point is not in the effective stable area, the resultant ground reaction force point is in the effective stable area, and the real-time correction amount Δθ _a (t) is calculated according to the formula (1) in Embodiment 1;

When the resultant force point of the ground reaction force is not in the effective stable area, the planning ZMP point is in the effective stable area, and the real-time correction amount Δθ _a (t) is calculated according to (2) formula in embodiment 1;

When the planned ZMP point and the resultant point of ground reaction force are not in the effective stable area, the real-time correction value Δθ _a (t) is calculated according to the formula (3) in Embodiment 1.

The servo driver 503 is used to drive the ankle joint of the robot after adding the dynamic gait θ _a0 (t) in the feedforwarder 501 and the real-time correction amount Δθ _a (t) in the real-time corrector 502 . It should be noted that the ankle joints of both feet of the robot are respectively equipped with the above-mentioned systems, and the robot is divided into a single-foot support period and a double-foot support period. The ankle joint is corrected; when in the double-foot support phase, the system of the ankle joints of both feet corrects the respective ankle joints of both feet.

In the system shown in the embodiment of the present invention, by correcting the ankle joint angle of the robot in real time, the planned ZMP point and the resultant force point of the ground reaction force are all within the effective stable area, and the stable walking of the robot is realized; and no specific robot is required. The dynamic model is simple to calculate, and can quickly correct the dynamic gait of the robot, so that the robot can adapt to the unknown environment.

All or part of the technical solutions provided by the above embodiments can be realized by software programming, and the software program is stored in a readable storage medium, such as a hard disk, an optical disk or a floppy disk in a computer.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the protection of the present invention. within range.

Claims (14)

1. A control method for humanoid robot stable walking based on an effective stable region is characterized by comprising the following steps:

measuring the ground reaction force of the robot through a force sensor;

according to the ground reaction force, obtaining a ground reaction force resultant point of the robot;

judging whether a planned zero moment point and the resultant point of the ground reaction force are both in an effective stable area;

and controlling the robot to walk according to the judgment result.

2. The method for controlling the humanoid robot to walk stably based on the effective stable region of claim 1, wherein the controlling the robot to walk according to the determination result specifically comprises:

and if the judgment result shows that the planning zero moment point and the ground reaction force resultant point are both in the effective stable area, the robot walks according to the planned dynamic gait.

3. The method for controlling the humanoid robot to walk stably based on the effective stable region of claim 1, wherein the controlling the robot to walk according to the determination result specifically comprises:

and if the judgment result shows that the planned zero moment point is not in the effective stable area and the resultant force point of the ground reaction force is in the effective stable area, correcting the ankle joint angle of the supporting leg according to the distance between the planned zero moment point and the boundary of the effective stable area.

4. The method for controlling the humanoid robot to walk stably based on the effective stable region as claimed in claim 3, wherein the correcting according to the distance between the planned zero moment point and the boundary of the effective stable region specifically comprises:

dynamic gait theta planned off-line_a0(t) adding a real-time correction amount [ Delta ] [ theta ] to the correction signal_a(t)；

The real-time correction amount Delta theta_aThe (t) is specifically:

<math> <mrow> <msub> <mi>&Delta;&theta;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mo>&Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mi>&phi;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>dt</mi> </mrow></math>

wherein, <math> <mrow> <mi>&phi;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mrow> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>K</mi> <mrow> <mn>1</mn> <mi>c</mi> </mrow> </msub> <mo>*</mo> <msub> <mi>d</mi> <mi>zmp</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>,</mo> </mtd> <mtd> <msub> <mi>F</mi> <mi>foot</mi> </msub> <mo>></mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mrow> <mo>-</mo> <mi>K</mi> </mrow> <mrow> <mn>1</mn> <mi>r</mi> </mrow> </msub> <mo>*</mo> <msub> <mi>&Delta;&theta;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>-</mo> <mi>T</mi> <mo>)</mo> </mrow> <mo>,</mo> </mtd> <mtd> <msub> <mi>F</mi> <mi>foot</mi> </msub> <mo>=</mo> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> <mo>,</mo> </mrow></math> d_zmp(t) represents the distance between the planned zero moment point and the effective stable area boundary at the moment t, K_1cAnd K_1rIs a coefficient, and 0<K_1c<1、0<K_1r<1, T is the computer control period, F_footIs the magnitude of the ground reaction force to the foot measured by the force sensor, F_foot>0 indicates that the foot is in a supporting state, F_foot0 indicates that the foot is in a swing state.

5. The method for controlling the humanoid robot to walk stably based on the effective stable region of claim 1, wherein the controlling the robot to walk according to the determination result specifically comprises:

and if the judgment result is that the planning zero moment point is in the effective stable area and the ground reaction force resultant point is not in the effective stable area, correcting the ankle joint angle of the supporting leg according to the distance between the ground reaction force resultant point and the boundary of the effective stable area.

6. The method for controlling the humanoid robot to walk stably based on the effective stable area as claimed in claim 5, wherein the correcting according to the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area specifically comprises:

dynamic gait theta planned off-line_a0(t) adding a real-time correction amount [ Delta ] [ theta ] to the correction signal_a(t)；

The real-time correction amount Delta theta_aThe (t) is specifically:

wherein,

d_f(t) represents the distance between the resultant point of the ground reaction force and the boundary of the effective stable area at the moment t, K_2cAnd K_2rIs a coefficient, and 0<K_2c<1、0<K_2r<1, T is the computer control period, F_footIs the ground to the foot measured by the force sensorMagnitude of reaction force, F_foot>0 indicates that the foot is in a supporting state, F_foot0 indicates that the foot is in a swing state.

7. The method for controlling the humanoid robot to walk stably based on the effective stable region of claim 1, wherein the controlling the robot to walk according to the determination result specifically comprises:

and if the judgment result shows that the planning zero moment point and the ground reaction force resultant point are not in the effective stable area, correcting the ankle joint angle of the supporting leg according to the product of the distance between the planning zero moment point and the effective stable area boundary and the distance between the ground reaction force resultant point and the effective stable area boundary.

8. The method for controlling the humanoid robot to walk stably based on the effective stable region of claim 7, wherein the correcting is performed according to a product of a distance between the planned zero moment point and the boundary of the effective stable region and a distance between the resultant force point of the ground reaction force and the boundary of the effective stable region, and specifically comprises:

dynamic gait theta planned off-line_a0(t) adding a real-time correction amount [ Delta ] [ theta ] to the correction signal_a(t)；

The real-time correction amount Delta theta_aThe (t) is specifically:

<math> <mrow> <msub> <mi>&Delta;&theta;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>=</mo> <msubsup> <mo>&Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mi>&psi;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>dt</mi> </mrow></math>

wherein, <math> <mrow> <mi>&psi;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mrow> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>K</mi> <mrow> <mn>3</mn> <mi>c</mi> </mrow> </msub> <mo>*</mo> <msub> <mi>d</mi> <mi>zmp</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>*</mo> <msub> <mi>d</mi> <mi>f</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>,</mo> </mtd> <mtd> <msub> <mi>F</mi> <mi>foot</mi> </msub> <mo>></mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mrow> <mo>-</mo> <mi>K</mi> </mrow> <mrow> <mn>3</mn> <mi>r</mi> </mrow> </msub> <mo>*</mo> <msub> <mi>&Delta;&theta;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>-</mo> <mi>T</mi> <mo>)</mo> </mrow> <mo>,</mo> </mtd> <mtd> <msub> <mi>F</mi> <mi>foot</mi> </msub> <mo>=</mo> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> <mo>,</mo> </mrow></math> d_zmp(t) represents the distance of the planned zero moment point from the effective stable area boundary at time t,d_f(t) represents the distance between the resultant point of the ground reaction force and the boundary of the effective stable area at the moment t, K_3cAnd K_3rIs a coefficient, and 0<K_3c<1、0<K_3r<1, T is the computer control period, F_footIs the magnitude of the ground reaction force to the foot measured by the force sensor, F_foot>0 indicates that the foot is in a supporting state, F_foot0 indicates that the foot is in a swing state.

9. A control device for humanoid robot stable walking based on effective stable region, which is characterized in that the device comprises:

the force sensor is used for measuring the ground reaction force of the robot;

the processing module is used for obtaining a resultant force point of the ground reaction force of the robot according to the ground reaction force measured by the force sensor;

the judging module is used for judging whether a planning zero moment point and a resultant point of the ground reaction force obtained by the processing module are both in an effective stable area;

and the control module is used for controlling the robot to walk according to the judgment result of the judgment module.

10. The control device for the humanoid robot to walk stably based on the effective stable region as claimed in claim 9, wherein the control module specifically comprises:

and the processing unit is used for enabling the robot to walk according to a planned dynamic gait when the judgment result of the judgment module is that the planned zero moment point and the ground reaction force resultant point are both in the effective stable area.

11. The control device for the humanoid robot to walk stably based on the effective stable region as claimed in claim 9, wherein the control module specifically comprises:

and the first correcting unit is used for correcting the ankle joint angle of the supporting leg according to the distance between the planned zero moment point and the boundary of the effective stable area when the judgment result of the judging module is that the planned zero moment point is not in the effective stable area and the resultant force point of the ground reaction force is in the effective stable area.

12. The control device for the humanoid robot to walk stably based on the effective stable region as claimed in claim 9, wherein the control module specifically comprises:

and the second correction unit is used for correcting the ankle joint angle of the supporting leg according to the distance between the resultant force point of the ground reaction force and the boundary of the effective stable area when the judgment result of the judgment module is that the planned zero moment point is in the effective stable area and the resultant force point of the ground reaction force is not in the effective stable area.

13. The control device for the humanoid robot to walk stably based on the effective stable region as claimed in claim 9, wherein the control module specifically comprises:

and the third correcting unit is used for correcting the ankle joint angle of the supporting leg according to the product of the distance between the planned zero moment point and the effective stable area boundary and the distance between the ground reaction force resultant point and the effective stable area boundary when the judging result of the judging module shows that the planned zero moment point and the ground reaction force resultant point are not in the effective stable area.

14. A control system for humanoid robot stable walking based on effective stable region, characterized in that the system includes:

a feedforward for providing a dynamic gait θ of the robot for off-line planning_a0(t)；

A real-time corrector for providing a dynamic gait trajectory theta to the feedforward device when the ground reaction force resultant point and/or the planned zero moment point of the robot are not within the effective stable area_a0(t) real-time correction amount [ Delta ] [ theta ] for correction_a(t)；

A servo driver for driving a dynamic gait theta in the feedforward_a0(t) and a real-time correction amount Delta theta in the real-time corrector_a(t) after the addition, driving the ankle joint of the robot.

Images