CN100594459C - Foot touching-ground time control method and system for stable walk of human imitating robert - Google Patents

Abstract

The invention discloses a method for controlling the time of foot landing which can lead a humanoid robot walk steadily and a system thereof, belonging to the field of robots. The method comprises that: the feet of the swing legs of a robot are detected to judge whether the feet land or not; if the feet of the swing legs of the robot land before the time of the programming pace, the ankle of the swing legs is elevated; if the feet of the swing legs of the robot land after the time of the programming pace, the ankle of the swing legs is lowered. The system comprises a detection device and an adjustment device; wherein, the adjustment device comprises an advanced adjustment module, a lagging adjustment module and a supporting adjustment module. The invention has the effect that the robot canwalk steadily in the actual environment by amending the programming dynamic pace of the robot and controlling the pace in real time.

Description

A kind of control method and system of pin touchdown time of apery robot stabilized walking

Technical field

The present invention relates to the robot field, particularly a kind of control method and system of pin touchdown time of apery robot stabilized walking.

Background technology

Bipod walking robot is the same with the people, is to move by the walking realization of two legs.In order to make robot ambulation, need the run trace (dynamically gait) of given robot.The dynamic gait of robot is the motion in a kind of intrinsic, cycle, produces according to biped robot's integral power.Because the coupling of constraint condition and the complicacy of kinetics equation, dynamically gait is calculated needs an optimizing process.So dynamically gait generally can only realize by offline computing method.That is to say that dynamically gait generally is to generate under the known situation of hypothesis biped robot's model and surrounding environment.But because there is error in the modeling of robot, and the variation of surrounding environment or produced unknown situation, if the dynamic gait of being planned is not revised in real time and controlled, robot produces instability possibly even abnormal occurrence such as falls down.Therefore, for make robot can be in actual environment stabilized walking, need the dynamic gait of planning be revised according to the current oneself state of current environmental information and robot, carry out real-time gait control, the change that overcomes environment is with uncertain.

Prior art one discloses a kind of control method based on ZMP (Zero Moment Point, zero moment point) compensation.In order to make robot two sufficient gait stabilities, utilize ZMP that robot is compensated.In ZMP, the resultant moment that ground reaction force and gravity produced on acting at the bottom of the robot foot equals zero.Realize the ZMP compensation by the upper body and the correction robot foot position that change robot.

Prior art two discloses a kind of method of controlling two-foot walking robot, according to ground reaction force, calculates roll angle and pitch angle.Drive detent by this roll angle and pitch angle, normal vector aligns with the reference direction on the gravity direction.

After prior art was studied, the inventor found: the robotary correction in the prior art one need be carried out complicated dynamics calculation.And because whether the touchdown time of leading leg of robot decision robot begins shift weight, so the robot foot touchdown time has material impact to the stability of robot.But this technology is not considered this factor, thereby causes robot still can produce instability even abnormal occurrence such as fall down.Equally, prior art two is not taken into account the robot foot touchdown time yet to the material impact of the stability of robot, so it is unstable even abnormal occurrence such as fall down robot also can to occur.

Summary of the invention

For make robot can be in actual environment stabilized walking, the embodiment of the invention provides a kind of control method and system of pin touchdown time of apery robot stabilized walking.Described technical scheme is as follows:

A kind of control method of pin touchdown time of apery robot stabilized walking, described method comprises:

Whether the pin that detection machine people leads leg lands and the size of contact force;

If the pin that robot leads leg lands in advance than the pin touchdown time of planning expectation, then according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg;

If the pin that robot leads leg lands than the pin touchdown time hysteresis of planning expectation, then the ankle-joint of leading leg is lowerd.

A kind of control system of pin touchdown time of apery robot stabilized walking, described system comprise checkout equipment and adjustment equipment:

Described checkout equipment: the pin that is used for the force transducer detection machine people by the robot anklebone place whether land and pin and ground between the size of contact force;

Described adjustment equipment: be used for by adjusting foot of robot touchdown time, make it and plan that the pin touchdown time of expectation coincide, if the pin that robot leads leg lands in advance than the pin touchdown time of planning expectation, then according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg; If the pin that robot leads leg lands than the pin touchdown time hysteresis of planning expectation, then the ankle-joint of leading leg is lowerd.

Described adjustment equipment comprises adjusting module, hysteresis adjusting module and support adjusting module in advance:

Shift to an earlier date adjusting module: be used for working as F _Foot＞0 and t＜T _LandThe time, then the robot pin of leading leg lands in advance than the pin touchdown time of planning expectation, robot according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg, the described calculating robot real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=Δ Z _f(t-T)+K ₁F _Foot

Hysteresis adjusting module: be used for working as F _Foot=0 and t＜T _LandThe time, then the robot pin of leading leg lags behind than the pin touchdown time of planning expectation and lands, and judges to lead leg robot whether pin can land when stretching;

If can land, the lead leg height of ankle-joint of the robot of loweing; The described calculating robot real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=Δ Z _f(t-T)-h;

Sole still can't kiss the earth if robot leads leg when stretching fully, then adjusts hip joint, knee joint and the ankle-joint of other one leg, and it is crooked that this leg is increased, and robot upper body height descends, so that the pin of leading leg can reach ground;

Support adjusting module: be used for working as F _Foot〉=M _rAnd t＜T _LandThe time, then this is led leg and supports separately entire machine people, and calculating robot's real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=(1-K ₂) Δ Z _f(t-T);

Described T is the sampling period of computer servo control system, K ₁And K ₂Be coefficient, 0＜K ₁＜1,0＜K ₂＜1, F _FootBe the pin that records of force transducer and the contact force between ground, T _LandBe the pin touchdown time of planning expectation, Mr is the threshold value of switching controls mode, and h is the constant greater than zero.

The beneficial effect of the technical scheme that the embodiment of the invention provides is:

Realize robot stabilized walking by revising biped robot's pin touchdown time, need not concrete robot dynamics's model, calculate simple, be swift in response, the height of rapid adjustment robot foot, make robot can be on broken terrain stabilized walking, and this technical scheme is not at particular machines people structure, wide adaptability.Both combine dynamic gait, and guaranteed to realize walking step state, and can keep mobile equilibrium again.

Description of drawings

In order to be illustrated more clearly in the embodiment of the invention or technical scheme of the prior art, to do to introduce simply to the accompanying drawing of the required use of embodiment below, apparently, accompanying drawing in describing below only is some embodiments of the present invention, for those of ordinary skills, under the prerequisite of not paying creative work, can also obtain other accompanying drawing according to these accompanying drawings.

Fig. 1 is the walking controller structural drawing that the embodiment of the invention 1 provides;

Fig. 2 is a kind of process flow diagram of control method of the pin touchdown time of a kind of apery robot stabilized walking of providing of the embodiment of the invention 1;

Fig. 3 is land in advance an adjustment schematic diagram under the situation of robot foot that the embodiment of the invention 1 provides;

Fig. 4 is land in advance a kind of process flow diagram under the situation of robot foot that the embodiment of the invention 1 provides;

Fig. 5 is lag behind an adjustment schematic diagram under the situation that lands of the robot foot that provides of the embodiment of the invention 1;

Fig. 6 is lag behind a kind of process flow diagram under the situation that lands of the robot foot that provides of the embodiment of the invention 1;

Fig. 7 is the structural drawing of the control system that provides of the embodiment of the invention 2;

Fig. 8 is the structural drawing of the adjustment equipment that provides of the embodiment of the invention 2.

Embodiment

For making the purpose, technical solutions and advantages of the present invention clearer, embodiment of the present invention is described further in detail below in conjunction with accompanying drawing.Obviously, described embodiment only is the present invention's part embodiment, rather than whole embodiment.Based on the embodiment among the present invention, those of ordinary skills belong to the scope of protection of the invention not making the every other embodiment that is obtained under the creative work prerequisite.

Whether the force transducer detection machine people pin of the present invention by the robot anklebone place lands and the size of contact force, by adjusting foot of the robot touchdown time of expectation, makes it and the touchdown time planned coincide.

Embodiment 1

As shown in Figure 1, the embodiment of the invention provides a kind of robot walking controller.This robot walking controller adopts double structure, by the off-line gait planning Z that plays feedforward action _F0(t) and play the Δ Z of LOCAL FEEDBACK effect _f(t) constitute.Walking robot comprises the force transducer 6 and the web member thereof at upper body 1, hip joint 2, knee joint 3, ankle-joint 4, pin 5, ankle place, and the two legs of the lower part of the body respectively has 6 degree of freedom respectively.

When robot walks in real time, the reference altitude Z of robot anklebone _f(t) equal the dynamic gait Z that off-line is planned _F0(t) add real-time correction amount Z _f(t).Wherein, Z _F0(t) generally be under the known situation of given robot model and surrounding environment, to generate.Δ Z _f(t) be not land (in advance or lag behind land), in order to overcome uncertain and real-time correction ankle-joint height when making robot keep balance of environment in the time of expectation when robot foot.

Referring to Fig. 2, the embodiment of the invention provides a kind of control method of pin touchdown time of apery robot stabilized walking, by adjusting foot of the robot touchdown time of expectation, makes it and the touchdown time planned coincide.Concrete steps are as follows:

Step 201: whether the pin that the force transducer detection machine people by the robot anklebone place leads leg lands and the size of contact force;

If land, then execution in step 202, otherwise, execution in step 204.

Step 202:F _Foot＞0 and t＜T _Land, the pin that robot leads leg lands in advance than the planning gait;

Step 203: robot raises this ankle-joint position of leading leg according to the pin contact force of leading leg with being in proportion; Execution in step 208 then;

Step 204:F _Foot=0, t＜T _Land, the pin that robot leads leg lags behind than the planning gait and lands;

Step 205: judge and lead leg robot whether sole can land when stretching;

If can land, then execution in step 206, otherwise, execution in step 207.

Step 206: the robot height of ankle-joint of leading leg of then need loweing; Execution in step 208 then;

Step 207: stretch and lead leg, adjust hip joint, knee joint and the ankle-joint of other one leg, it is crooked that this leg is increased, and robot upper body height descends, and consequently the pin of leading leg reaches ground, to keep the balance of robot body;

Step 208:F _Foot〉=M _r, this is led leg and supports the entire machine people separately, and then this ankle-joint position of leading leg returns to the value of intrinsic dynamic gait regulation gradually.

Further, the pin that the embodiment of the invention will be led leg to robot describes than the situation that the planning gait land in advance, and as shown in Figure 3, robot raises this ankle-joint position of leading leg according to the pin contact force of leading leg with being in proportion.Lead leg when this and can support the entire machine man-hour separately, then this ankle-joint position of leading leg returns to the value of intrinsic dynamic gait regulation gradually.The real-time correction amount Z of robot anklebone _f(t) calculate by following two formulas:

${\mathrm{\ΔZ}}_f\left(t\right)={\∫}_0^t\mathrm{\φ}\left(t\right)\mathrm{dt}---\left(1\right)$

$\mathrm{\&phi;}\left(t\right)=\left\{\begin{array}{cc}{K}_1*F_{\mathrm{foot}},& F_{\mathrm{foot}}>0,t<T_{\mathrm{land}}\\ -{\mathrm{<figure-callout\; id="2"\; label="K"\; filenames="C2008101719850012H1.png"\; state="\{\{state\}\}">K</figure-callout>}}_2*{\mathrm{\&Delta;Z}}_f(t-T),& F_{\mathrm{foot}}\&GreaterEqual;M_r,t>T_{\mathrm{land}}\end{array}\right.---\left(2\right)$

Wherein, T is the sampling period of computer servo control system.K ₁And K ₂Be coefficient, 0＜K ₁＜1,0＜K ₂＜1.F _FootBe the pin that records of force transducer and the contact force between ground.T _LandIt is the pin touchdown time of planning expectation.Mr is the threshold value of switching controls mode, its value be slightly less than robot body weight (for example, its value equal the robot body weight 80% to 95%).

Referring to Fig. 4, concrete steps are as follows:

Step 401:F _Foot＞0 and t＜T _Land, the pin that robot leads leg is adjusted than the situation that the planning gait lands in advance;

Step 402: pin that robot leads leg lands in advance than the planning gait, robot then according to being in proportion of pin contact force raise the position of this ankle-joint of leading leg;

Particularly, pin that robot leads leg lands in advance than the planning gait, and calculating robot's real-time correction of ankle-joint of should leading leg is:

ΔZ _f(t)＝ΔZ _f(t-T)+K ₁F _foot

Step 403:F _Foot〉=M _rAnd t＞T _Land, robot leads leg and supports separately the entire machine people, and then the robot ankle-joint position of should leading leg returns to the value of intrinsic dynamic gait regulation gradually.

Concrete, robot leads leg and supports the entire machine people separately, and calculating robot's real-time correction of ankle-joint of should leading leg is:

ΔZ _f(t)＝(1-K ₂)ΔZ _f(t-T)

Further, the embodiment of the invention will describe than the situation that the hysteresis of planning gait lands the pin that robot leads leg, and as shown in Figure 5, robot will suitably lower the leg ankle-joint, wherein, the degree of loweing will depend on the discrepancy in elevation between the surface of contact of actual contact face and expection.Lead leg when this and can support the entire machine man-hour separately, then this ankle-joint position of leading leg returns to the value of intrinsic dynamic gait regulation gradually.In such cases, the real-time correction amount Z of robot anklebone _f(t) calculate by following two formulas:

As shown in Figure 6, concrete steps are as follows:

Step 601:F _Foot=0, t＜T _Land, the pin that robot leads leg is adjusted than the situation that the hysteresis of planning gait lands;

Step 602: pin that robot leads leg lags behind than the planning gait and lands, and judges to lead leg robot whether pin can land when stretching;

If can land, then execution in step 603, otherwise, execution in step 604;

Step 603: pin that robot leads leg lags behind than the planning gait and lands, the lead leg height of ankle-joint of the robot of loweing, and the degree of loweing depends on the discrepancy in elevation between the surface of contact of actual contact face and expection; Execution in step 605 then;

Particularly, pin that robot leads leg lags behind than the planning gait and lands, and calculating robot's real-time correction of ankle-joint of should leading leg is:

ΔZ _f(t)＝ΔZ _f(t-T)-h

Step 604: robot leads leg when stretching fully sole still can't kiss the earth, then adjust hip joint, knee joint and the ankle-joint of other one leg, it is crooked that this leg is increased, and robot upper body height descends, so that the pin of leading leg reaches ground, to keep the balance of robot body;

Step 605:F _Foot〉=M _rAnd t＞T _Land, then this is led leg and supports the entire machine people separately, and then this ankle-joint position of leading leg returns to the value of intrinsic dynamic gait regulation gradually.

Particularly, support separately the entire machine people when robot leads leg, then calculating robot's real-time correction of ankle-joint of should leading leg is:

ΔZ _f(t)＝(1-K ₂)ΔZ _f(t-T)

The embodiment of the invention need not concrete robot dynamics's model, calculate simple, reflection rapidly, the height of rapid adjustment robot foot, make robot can be on broken terrain stabilized walking, and not at particular machines people structure, wide adaptability had both combined dynamic gait, guaranteed to realize walking step state, can keep mobile equilibrium again, well solve the robot problem of unstable.

Embodiment 2

Referring to Fig. 7, the present invention also provides a kind of control system of pin touchdown time of apery robot stabilized walking, specifically comprises:

Checkout equipment 701: whether the pin that is used for the force transducer detection machine people by the robot anklebone place lands and the size of contact force;

Adjustment equipment 702: be used for by adjusting foot of the robot touchdown time of expectation, make it and the touchdown time planned coincide,, then raise this ankle-joint position of leading leg if the pin that robot leads leg lands in advance than the planning gait; If the pin that robot leads leg lags behind than the planning gait and lands, then the ankle-joint of leading leg is lowerd.

Further, referring to Fig. 8, adjustment equipment 702 comprises:

Shift to an earlier date adjusting module 801: be used for working as F _Foot＞0 and t＜T _LandThe time, then the robot pin of leading leg lands in advance than the planning gait, robot according to being in proportion of pin contact force raise the position of this ankle-joint of leading leg, the described calculating robot real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=Δ Z _f(t-T)+K ₁F _Foot

Hysteresis adjusting module 802: be used for working as F _Foot=0, t＜T _LandThe time, then the robot pin of leading leg lags behind than the planning gait and lands, and judges to lead leg robot whether pin can land when stretching;

If can land, the lead leg height of ankle-joint of the robot of loweing; The described calculating robot real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=Δ Z _f(t-T)-h;

Sole still can't kiss the earth if robot leads leg when stretching fully, then adjusts hip joint, knee joint and the ankle-joint of other one leg, and it is crooked that this leg is increased, and robot upper body height descends, so that the pin of leading leg reaches ground;

Support adjusting module 803: be used for working as F _Foot〉=M _rAnd t＞T _LandThe time, then this is led leg and supports separately entire machine people, and calculating robot's real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=(1-K ₂) Δ Z _f(t-T).

The embodiment of the invention need not concrete robot dynamics's model, calculate simple, reflection rapidly, the height of rapid adjustment robot foot, make robot can be on broken terrain stabilized walking, and not at particular machines people structure, wide adaptability had both combined dynamic gait, guaranteed to realize walking step state, can keep mobile equilibrium again, well solve the robot problem of unstable.

All or part of content in the technical scheme that above embodiment provides can realize that its software program is stored in the storage medium that can read by software programming.

The above only is preferred embodiment of the present invention, and is in order to restriction the present invention, within the spirit and principles in the present invention not all, any modification of being done, is equal to replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (8)

1, a kind of control method of pin touchdown time of apery robot stabilized walking is characterized in that, described method comprises:

Whether the pin that detection machine people leads leg lands;

If the pin that described robot leads leg lands in advance than the pin touchdown time of planning expectation, then according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg;

If the pin that described robot leads leg lands than the pin touchdown time hysteresis of planning expectation, then the ankle-joint of leading leg is lowerd.

2, the control method of the pin touchdown time of apery robot stabilized walking as claimed in claim 1, it is characterized in that, if the pin that described robot leads leg lands in advance than the pin touchdown time of planning expectation, then according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg, specifically comprise:

If F _Foot＞0 and t＜T _Land, then the robot pin of leading leg lands in advance than the pin touchdown time of planning expectation, robot according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg, the described calculating robot real-time correction of ankle-joint of leading leg is: Δ Z _f(t)=Δ Z _f(t-T)+K ₁F _Foot

The size of the contact force between described pin and ground obtains described K by the force transducer detection at robot anklebone place ₁Be coefficient, 0＜K ₁＜1, F _FootBe the pin that records of force transducer and the contact force between ground, T _LandBe the pin touchdown time of planning expectation, T is the sampling period of computer servo control system.

3, the control method of the pin touchdown time of apery robot stabilized walking as claimed in claim 1 is characterized in that, if the pin touchdown time that the pin that described robot leads leg is expected than planning lags behind lands, and then the ankle-joint of leading leg is lowerd, and specifically comprises:

If F _Foot=0 and t＜T _Land, then the robot pin of leading leg lags behind than the pin touchdown time of planning expectation and lands, and judges to lead leg robot whether pin can land when stretching;

If can land, the lead leg height of ankle-joint of the robot of loweing, the described calculating robot real-time correction of ankle-joint of leading leg is: Δ Z _f(t)=Δ Z _f(t-T)-h;

Sole still can't kiss the earth if robot leads leg when stretching fully, then adjusts hip joint, knee joint and the ankle-joint of other one leg, and it is crooked that this leg is increased, and robot upper body height descends, so that the pin of leading leg reaches ground;

Described F _FootBe the pin that records of force transducer and the contact force between ground, T _LandBe the pin touchdown time of planning expectation, T is the sampling period of computer servo control system, and h is the constant greater than zero.

4, as the control method of the pin touchdown time of claim 2 or 3 described apery robot stabilized walkings, it is characterized in that described method also comprises:

If F _Foot〉=M _rAnd t＜T _Land, then this is led leg and supports separately entire machine people, and calculating robot's real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=(1-K ₂) Δ Z _f(t-T);

Described K ₂Be coefficient, 0＜K ₂＜1, M _rIt is the threshold value of switching controls mode.

5, a kind of control system of pin touchdown time of apery robot stabilized walking is characterized in that, described system comprises checkout equipment and adjustment equipment:

Described checkout equipment: the pin that is used for the force transducer detection machine people by the robot anklebone place whether land and pin and ground between the size of contact force;

Described adjustment equipment: be used for by adjusting foot of robot touchdown time, make it and plan that the pin touchdown time of expectation coincide, if the pin that robot leads leg lands in advance than the pin touchdown time of planning expectation, then according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg; If the pin that robot leads leg lands than the pin touchdown time hysteresis of planning expectation, then the ankle-joint of leading leg is lowerd.

6, the control system of the pin touchdown time of apery robot stabilized walking as claimed in claim 5 is characterized in that, described adjustment equipment specifically comprises:

Shift to an earlier date adjusting module: be used for working as F _Foot＞0 and t＜T _LandThe time, then the robot pin of leading leg lands in advance than the pin touchdown time of planning expectation, robot according to being in proportion of the contact force between pin and ground raise this ankle-joint position of leading leg, the described calculating robot real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=Δ Z _f(t-T)+K ₁F _Foot

The size of the contact force between described pin and ground obtains described F by the force transducer detection at robot anklebone place _FootBe the pin that records of force transducer and the contact force between ground, T _LandBe the pin touchdown time of planning expectation, T is the sampling period of computer servo control system, K ₁Be coefficient, 0＜K ₁＜1.

7, the control system of the pin touchdown time of apery robot stabilized walking as claimed in claim 5 is characterized in that, described adjustment equipment specifically comprises:

Hysteresis adjusting module: be used for working as F _Foot=0 and t＜T _LandThe time, then the robot pin of leading leg lags behind than the pin touchdown time of planning expectation and lands, and judges to lead leg robot whether pin can land when stretching;

If can land, the lead leg height of ankle-joint of the robot of loweing; The described calculating robot real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=Δ Z _f(t-T)-h;

Sole still can't kiss the earth if robot leads leg when stretching fully, then adjusts hip joint, knee joint and the ankle-joint of other one leg, and it is crooked that this leg is increased, and robot upper body height descends, so that the pin of leading leg reaches ground;

Described F _FootBe the pin that records of force transducer and the contact force between ground, T _LandBe the pin touchdown time of planning expectation, T is the sampling period of computer servo control system, and h is the constant greater than zero.

8, as the control system of the pin touchdown time of claim 6 or 7 described apery robot stabilized walkings, it is characterized in that described adjustment equipment also comprises:

Support adjusting module: be used for working as F _Foot〉=M _rAnd t＜T _LandThe time, then this is led leg and supports separately entire machine people, and calculating robot's real-time correction of ankle-joint of should leading leg is: Δ Z _f(t)=(1-K ₂) Δ Z _f(t-T);

Described K ₂Be coefficient, 0＜K ₂＜1, Mr is the threshold value of switching controls mode.

Images