CN105242677A - Quadruped robot biped support phase force hybrid force control method - Google Patents

Abstract

The invention provides a quadruped robot biped support phase force hybrid force control method. The method comprises the steps of (S1) projecting the overall movement of a robot to a radial plane and a normal plane, (S2) establishing a control model, simplifying the movement of the robot along the radial plane as a plane seven-connection-rod model, and simplifying the movement along the normal plane as a linear inverted pendulum model, and simplifying the plane seven-connection-rod model as a plane virtual telescoping leg model, wherein the control target of the plane virtual telescoping leg model is a mass center height, a body pitch angle and a horizontal displacement, (S3) carrying out hybrid controlling according to a control model, establishing the kinetic equation of the plane virtual telescoping leg model and establishing the kinetic equation through a Newton-Euler method, controlling the mass center height and the body pitch angle through a position servo method, and using the double-ring control method of outer ring position and inner ring foot end force for the horizontal displacement of a virtual telescoping leg plane model. The method has the advantages of good control effect and the improvement of robot adaptability.

Description

Quadruped robot biped supports phase place force-location mix control method

Technical field

The present invention is mainly concerned with motion planning and robot control technical field, refers in particular to a kind of biped being applicable to quadruped robot and supports phase place force-location mix control method.

Background technology

Mobile robot can arrive the place that the mankind cannot arrive or should not arrive because of environmental hazard, four-leg bionic robot is a class legged type robot of the motor pattern copying quadruped mammal, because it complicated landform can have very strong adaptive faculty on rock, abrupt slope etc., the task such as Material Transportation, patrol can be carried out under non-structure environment, and the mankind can be replaced to carry out risky operation, thus there is great researching value.

Quadruped robot is generally made up of four bionic legs and a body, and every bar leg comprises a side direction joint and at least two forward direction joints.Learn by imitation the mode of motion of occurring in nature quadruped robot, and the mode of motion of current quadruped robot mainly contains three kinds of gaits: TROT gait (trot gait), BOUND gait (gait of running) and WALK gait (Crawl gait).

The control of quadruped robot is typical floating foundation control problem, and control objectives is generally its this posture, and its this posture controls primarily of supporting leg.Because quadruped robot degree of freedom is various, realize controlling very complicated to the entirety of robot, therefore need suitably to simplify model.

When carrying out modeling to quadruped robot, multi link areal model and SLIP model are relatively more conventional at present and effective simplified model, these two models all can embody the comprehensive kinematic and dynamic modeling of trot gait, and System design based on model method carrys out CONTROLLER DESIGN according to its kinematics and kinetics equation mostly.

Servocontrol rate is adopted based on kinematic control method, each joint position information is calculated according to sensor information and control objectives, and realize the tracking to desired trajectory by position servo rule, because position servo control has larger rigidity, thus the load capacity that robot is stronger can be made, but easily occur leading leg under unstructured moving grids and shift to an earlier date or delayed situation of landing, in this case the unexpected impact produced is very large on the impact of this posture, therefore needs algorithm for design to impact to adapt to larger pose.

The multiple feedback loop methods such as factored moment method are generally adopted based on dynamic (dynamical) control method, calculate foot by robot body posture information and hold expected force, and control to hold expected force, and then reach the object of control machine human body pose enough by calculating joint driven torque.This control method has good compliance, has stronger adaptive faculty to unstructured moving grids.But, its shortcoming is may produce internal force between two supporting legs, when supporting leg be switched to lead leg pattern when, internal force abrupt release, to to lead leg and the attitude of body produces unexpected impact, and the rigidity of driver is larger relative to position control by the impact of total quality.Therefore, force control method needs force distribution method reasonable in design, can strengthen the locomitivity of robot while control pose.

Summary of the invention

The technical problem to be solved in the present invention is just: the technical matters existed for prior art, the invention provides a kind of control effects quadruped robot biped that is good, that can improve robot adaptive faculty and supports phase place force-location mix control method.

For solving the problems of the technologies described above, the present invention by the following technical solutions:

A kind of quadruped robot biped supports phase place force-location mix control method, the steps include:

S1: the mass motion of robot is projected on sagittal plane and Normal plane; Described sagittal plane is to support pin and perpendicular to the plane of surface level; Described Normal plane is by body barycenter and perpendicular to the plane of sagittal plane;

S2: set up Controlling model; Motion on Robot sagittal plane is reduced to plane seven-link assembly model, and the motion on Normal plane is reduced to linear inverted pendulum model; Then be planar virtual Retractive leg model by the model simplification of plane seven-link assembly; The control objectives of planar virtual Retractive leg model is height of center of mass, the body angle of pitch and horizontal shift;

S3: carry out Hybrid mode according to Controlling model; Set up the kinematical equation of planar virtual Retractive leg model and set up its kinetics equation by Newton-Euler method, position servo control method is passed through for height of center of mass and the body angle of pitch, power control mode is adopted to the horizontal shift of virtual Retractive leg areal model.

As a further improvement on the present invention: in step s3, control body height of center of mass and the angle of pitch by leg is long, control body horizontal shift, to realize controlling the Approximate Decoupling of this posture by level of control foot end power.

As a further improvement on the present invention: adopt classical PID controller as the outer ring controller of power, supporting leg foot termination touch is made accurately to follow the tracks of expected force.

As a further improvement on the present invention: adopt Position Servo to control to realize the tracking to joint angles to knee joint and ankle-joint.

Compared with prior art, the invention has the advantages that:

1, the present invention is when quadruped robot Trot gait motion, can realize the accurate tracing control to this posture, and have stronger adaptability to discontinuous desired trajectory.

2, the power allocation strategy that the present invention is based on the proper pressure of foot effectively can avoid the phenomenon of robot supporting leg foot end slip in the process of walking, and the acceleration making robot stronger.

3, the present invention can improve the adaptive faculty of quadruped robot to unstructured moving grids, realizes robot in the ground walking of out-of-flatness.

4, the present invention can improve the load capacity of robot by increasing positioner rigidity.

5, clear in structure of the present invention, well arranged, has good theory value and engineering significance.

Accompanying drawing explanation

Fig. 1 is the structural representation of the present invention's quadruped robot platform in embody rule example.

Fig. 2 is the schematic diagram of the present invention's radial plane projection in embody rule example.

Fig. 3 is the uneoupled control block diagram of the present invention in the virtual Retractive leg structure of embody rule example midplane.

Fig. 4 is the schematic flow sheet of the inventive method.

Embodiment

Below with reference to Figure of description and specific embodiment, the present invention is described in further details.

Method of the present invention is mainly applicable to quadruped robot.As shown in Figure 1, be the system architecture of quadruped robot, be made up of body and four legs, every bar leg comprises a hip lateral rotation joint and three forward direction cradle heads (the front joint of hip, knee joint and ankle-joint).Each joint drives by hydraulic unit driver, and the displacement transducer be equipped with for detecting driver length and driving force and force snesor.Every bar leg foot end all installs three-dimensional force sensor, for the contact force information of measuring robots foot end with environment, and by the posture information of IMU measuring robots under inertial system.Above-mentioned quadruped robot has following mechanostructural property: (1) robot center of gravity is close to body geometric center; (2) quality of robot body is much larger than the quality of leg.

In the present invention, support for TROT gait biped, force-location mix control method when quadruped robot biped supports is described.Quadruped robot biped of the present invention supports phase place force-location mix control method, and it adopts the method for force-location mix control to carry out uneoupled control to robot body pose, to strengthen the adaptive faculty that robot changes non-structural ground and load.The present invention proposes the force distribution method rectifying pressure based on foot, this approach reduce the possibility that supporting leg foot end occurs to slide, and improve the acceleration of robot.

As shown in Figure 4, quadruped robot biped of the present invention supports phase place force-location mix control method, and step is:

S1: the mass motion of robot is projected on sagittal plane and Normal plane; Described sagittal plane is to support pin and perpendicular to the plane of surface level; Described Normal plane is by body barycenter and perpendicular to the plane of sagittal plane;

Because quadruped robot degree of freedom is various, kinematics and the kinetics equation of setting up robot entirety are comparatively numerous and diverse, therefore need quadruped robot to carry out Rational Simplification: trot gait is two repetition handoff procedures to pin holding state, therefore the mass motion of robot projects to by support pin and perpendicular to the sagittal plane of surface level by the present invention, and by barycenter and perpendicular to the Normal plane of sagittal plane.

S2: set up Controlling model;

In embody rule example, the motion on Robot sagittal plane can be reduced to plane seven-link assembly model, and the motion on Normal plane then can be reduced to linear inverted pendulum model.

Wherein, plane seven-link assembly model list leg has three forward direction cradle heads, because the kinematic and dynamic characteristic of single leg can be equivalent to Retractive leg structural model, is therefore the planar virtual model with Retractive leg structure by the model simplification of plane seven-link assembly again.The control objectives of planar virtual Retractive leg model is height of center of mass, the body angle of pitch and horizontal shift.

S3: carry out Hybrid mode according to Controlling model; Set up the kinematical equation of planar virtual Retractive leg model and set up its kinetics equation by Newton-Euler method, position servo control method is passed through for height of center of mass and the body angle of pitch, power control mode is adopted to the horizontal shift of virtual Retractive leg areal model.That is:

First, set up the kinematical equation of this model and set up its kinetics equation by Newton-Euler method.Because position control method has very large rigidity, therefore by position servo control method, the heavy burden ability of robot is improved for body height of center of mass and the angle of pitch.

Because planar virtual Retractive leg model is altogether containing 4 driving joints, and overall control objectives only has three, therefore needs interpolation constraint condition could realize controlling the entirety of model.

Because supporting leg horizontal direction foot end power is by the natural sulfur reservoir of cone of friction, one-piece construction is also by the Dynamic Constraints of ZMP point, in order to make virtual Retractive leg model avoid foot to hold slip, needing sufficient end power in the horizontal direction to add an equation of constraint, being the power assignment problem of horizontal direction.Therefore, power control mode is adopted to the horizontal shift of virtual Retractive leg areal model.

In embody rule example, the projection of quadruped robot in sagittal plane can be reduced to a seven-link assembly structure, as shown in Figure 2 (left front leg and right rear leg are as supporting leg).Wherein, (x, z) is the position of body barycenter under inertial coordinates system, and m is this weight, L ₀for robot body length, d is the width of body, l ₁, l ₂, l ₃be respectively the length of leg ankle-joint, knee joint, hip joint, θ ₁, θ ₂, θ ₃for left front leg ankle-joint angle, knee angle, hip joint angle, θ ₄, θ ₅, θ ₆for right rear leg ankle-joint angle, knee angle, hip joint angle, for the body angle of pitch, O, A, B, C, D are respectively system of world initial point (i.e. two supporting legs foot end line mid points), right rear leg hip and body tie point, left front leg hip and body tie point, the sufficient end points of right rear leg, the sufficient end points of left front leg, F _x1, F _z1, F _x2, F _z2be respectively the contact force that C point and D point are subject to.Because leg quality is very little relative to this weight, ignore leg quality and inertia when modeling.

Can leg be obtained by the schematic diagram on the left side in Fig. 2 long || AC|| and || BD|| is respectively:

$\left|\right|AC\left|\right|=\sqrt{{\left(l_1+l_{2}cos\left(\mathrm{\π}-{\mathrm{\θ}}_4\right)+l_3\cos \left({\mathrm{\θ}}_5-{\mathrm{\θ}}_4\right)\right)}^2+{(l_{2}sin(\mathrm{\π}-{\mathrm{\θ}}_4)+l_{3}sin({\mathrm{\θ}}_5-{\mathrm{\θ}}_4))}^2}---\left(1\right)$

$\left|\right|BD\left|\right|=\sqrt{{\left(l_1+l_{2}cos\left(\mathrm{\π}-{\mathrm{\θ}}_1\right)+l_3\cos \left({\mathrm{\θ}}_2-{\mathrm{\θ}}_1\right)\right)}^2+{(l_{2}sin(\mathrm{\π}-{\mathrm{\θ}}_1)+l_{3}sin({\mathrm{\θ}}_2-{\mathrm{\θ}}_1))}^2}---\left(2\right)$

From formula (1) ~ (2), leg is long || AC|| and || BD|| respectively with joint angles θ ₄, θ ₅and θ ₁, θ ₂corresponding.Therefore, from kinematics, in Fig. 2, the joint leg of left figure signal can be reduced to the virtual Retractive leg model shown in right figure, wherein α ₃, α ₄be respectively front and back leg incident angle.

Planar virtual Retractive leg structural kinetics model can be set up as follows by Fig. 2:

$y=\frac{L_{1}sin\left({\mathrm{\α}}_3\right)+L_{2}sin\left({\mathrm{\α}}_4\right)}{2}---\left(4\right)$

$x=\frac{L_{1}cos\left({\mathrm{\α}}_3\right)+L_{2}cos\left({\mathrm{\α}}_4\right)}{2}---\left(5\right)$

Its kinetics equation can be obtained equally as follows:

$f_{x1}+f_{x2}=m\stackrel{\·\·}{x}---\left(6\right)$

$f_{z1}+f_{z2}=mg+m\stackrel{\·\·}{z}---\left(7\right)$

Then have hip joint moment:

τ ₁＝L ₁sin(α ₃)f _x1-L ₁cos(α ₃)f _z1(9)

τ ₂＝L ₂sin(α ₄)f _x1-L ₂cos(α ₄)f _z1(10)

For kinetics equation (6) ~ (10), in the Retractive leg model in Fig. 2 shown in right figure and Fig. 2, the joint leg model shown in left figure is also equivalent.Therefore, all can the virtual Retractive leg model shown in the right figure being Fig. 2 by the plane seven-link assembly model simplification shown in the left figure of Fig. 2 from kinematics and dynamics aspect.

Based on the method for the invention described above, in embody rule example, need pose uneoupled control;

For incident angle α ₃, α ₄have:

0<α ₃<π,0<α ₄<π(11)

So have:

0<sin(α ₃)≤1,0<sin(α ₄)≤1(12)

-1<cos(α ₃)<1,-1<cos(α ₄)<1(13)

For forward direction displacement, there is singular point α by equation (5) is known ₃=α ₄=pi/2, is therefore not suitable for controlling forward direction displacement with leg is long.But for equation (3), (4), there is not singular point, therefore, can by the long l of leg ₁, l ₂controlling the body angle of pitch and height of center of mass, taking for simplicity by controlling carry out brief introduction and control the body angle of pitch method, and to make

(3), (4) are written as matrix form and are then:

$\left[\begin{array}{c}{S}_y\\ y\end{array}\right]=J_1\left[\begin{array}{c}{L}_1\\ L_2\end{array}\right]---\left(14\right)$

Wherein, $J_1=\left[\begin{array}{cc}\frac{sin\left({\mathrm{\&alpha;}}_3\right)}{2L_0}& -\frac{sin\left({\mathrm{\&alpha;}}_4\right)}{2L_0}\\ \frac{sin\left({\mathrm{\&alpha;}}_3\right)}{2}& \frac{\sin \left({\mathrm{\&alpha;}}_4\right)}{2}\end{array}\right].$

By formula (3) known J ₁nonsingular, and forward direction displacement on the impact of the body angle of pitch and height of center of mass by Jacobian matrix J ₁eliminate, therefore can to think kinematics Approximate Decoupling.

Control objectives due to plane Retractive leg structure has horizontal shift, height of center of mass and the body angle of pitch, and wherein height of center of mass and the body angle of pitch control by leg is long, and forward direction displacement controls to realize by sufficient end power.Only relevant to forward location in kinetics equation only have formula (6), needs the calculating that interpolation horizontal force distribution equations could realize sufficient end power, and then realize the control to horizontal shift.In order to the possibility making foot end occur to slide is minimum, adopts force distribution method as described below, and adopt double loop control to control.

The possibility that definition foot end occurs to slide is SCI (slip binding target):

$SCI=max\left(\frac{\left|F_x\right|}{\left|F_z\right|}\right)---\left(15\right)$

Wherein F _x, F _zfor the ground reaction force that robot is subject in gait processes.SCI value is less, and the possibility that foot end occurs to slide is less.For making SCI little as far as possible, then power allocation scheme can be adopted to be:

f _x1d/f _x2d＝f _z1/f _z2(16)

Simultaneous (6), (16) can expect that sufficient end power is:

$\left[\begin{array}{c}{f}_{x1d}\\ f_{x2d}\end{array}\right]=\left[\begin{array}{c}{f}_{z1r}/(f_{z1r}+f_{z2r})\\ f_{z2r}/(f_{z1r}+f_{z2r})\end{array}\right]m\stackrel{\&CenterDot;\&CenterDot;}{x}---\left(17\right)$

By during the foot end expected force of formula (17) calculated level direction and height of center of mass and the body angle of pitch completely irrelevant, therefore can think outer shroud, namely level foot end power and another two control objectives are Dynamics Decoupleds.

Must be controlled by joint moment for horizontal direction foot end power, can be obtained by (9), (10):

$\left[\begin{array}{c}{\mathrm{\&tau;}}_1\\ {\mathrm{\&tau;}}_2\end{array}\right]=J_2\left[\begin{array}{c}{f}_{x1}\\ f_{x2}\end{array}\right]+G---\left(18\right)$

Wherein, $J_2=\left[\begin{array}{cc}{L}_{1}sin\left({\mathrm{\&alpha;}}_3\right)& 0\\ 0& L_2\sin \left({\mathrm{\&alpha;}}_4\right)\end{array}\right],G=\left[\begin{array}{c}-L_{1}cos\left({\mathrm{\&alpha;}}_3\right)f_{z1}\\ -L_{2}cos\left({\mathrm{\&alpha;}}_4\right)f_{z2}\end{array}\right].$

By formula (3) known J ₂nonsingular, and pass through J ₂eliminate height of center of mass and the angle of pitch to the impact of joint moment with compensation term G, therefore can think that inner ring is dynamics Approximate Decoupling.

Adopt above-mentioned shown method, namely control body height of center of mass and the angle of pitch by leg is long, control body horizontal shift by level of control foot end power, can realize controlling the Approximate Decoupling of this posture.

Based on the method for the invention described above, in embody rule example, need to carry out Controller gain variations;

1) force controller design;

Based on formula (17), design classical PID controller is as the outer ring controller of power, and concrete form is as follows:

$\left[\begin{array}{c}{f^{\&prime;}}_{x1d}\\ {f^{\&prime;}}_{x2d}\end{array}\right]=m\left[\begin{array}{c}{f}_{z1r}/(f_{z1r}+f_{z2r})\\ f_{z2r}/(f_{z1r}+f_{z2r})\end{array}\right]\&lsqb;{\mathrm{Kp}}_x(x_d-x)+{\mathrm{Kd}}_x({\stackrel{\&CenterDot;}{x}}_d-\stackrel{\&CenterDot;}{x})+{\mathrm{Ki}}_x{\&Integral;}_0^t(x_d-x)d\mathrm{\&tau;}\&rsqb;---\left(19\right)$

Wherein, f' _x1d, f' _x2dfor the sufficient termination touch expected, x _dfor the position, x direction expected, f _z1r, f _z2rfor vertical direction foot holds power.

Because robot will keep balancing the constraint being subject to ZMP point at the volley, for virtual Retractive leg structure, for maintaining the biped holding state of robot, ZMP point need be limited on the line segment of biped line.Do not considering that in height of center of mass and the dynamic situation of the angle of pitch, this constraint type is as follows:

$k_1\frac{x_1-x}{z}g\&le;\frac{{\mathrm{Kp}}_x(x_d-x)+{\mathrm{Kd}}_x({\stackrel{\&CenterDot;}{x}}_d-\stackrel{\&CenterDot;}{x})+{\mathrm{Ki}}_x{\&Integral;}_0^t(x_d-x)d\mathrm{\&tau;}}{m}\&le;k_1\frac{x-x_2}{z}g---\left(20\right)$

Wherein, 0<k ₁<1, x ₁, x ₂be respectively D point and C point horizontal level.

In addition, foot end is by the natural sulfur reservoir condition of cone of friction: f' _x1d< μ f _z1r, f' _x1d< μ f _z1d, μ is friction factor, and controller becomes:

$f_{x1d}=\left\{\begin{array}{cc}{f^{\&prime;}}_{x1d}& -f_{x1\_max}<{f^{\&prime;}}_{x1d}<f_{x1\_max},f_{x1\_max}=k_2{\mathrm{\&mu;f}}_{z1r}\\ -f_{x1\_max}& {f^{\&prime;}}_{x1d}\&le;-f_{x1\_max},f_{x1\_max}=k_2{\mathrm{\&mu;f}}_{z1r}\\ f_{x1\_max}& {f^{\&prime;}}_{x1d}\&GreaterEqual;f_{x1\_max},f_{x1\_max}=k_2{\mathrm{\&mu;f}}_{z1r}\end{array}\right.---\left(21\right)$

$f_{x2d}=\left\{\begin{array}{cc}{f^{\&prime;}}_{x2d}& -f_{x2\_max}<{f^{\&prime;}}_{x2d}<f_{x2\_max},f_{x2\_max}=k_2{\mathrm{\&mu;f}}_{z2r}\\ -f_{x2\_max}& {f^{\&prime;}}_{x2d}\&le;-f_{x2\_max},f_{x2\_max}=k_2{\mathrm{\&mu;f}}_{z2r}\\ f_{x2\_max}& {f^{\&prime;}}_{x2d}\&GreaterEqual;f_{x2\_max},f_{x2\_max}=k_2{\mathrm{\&mu;f}}_{z2r}\end{array}\right.---\left(22\right)$

By regulating parameter Kp _x, Kd _x, Ki _xcan ensure while the displacement of control forward direction, avoid robot in motion process, occur the phenomenon overturn or foot end slides.

Based on formula (18), adopt classical PID controller that supporting leg foot termination touch can be made accurately to follow the tracks of expected force.

2) Design of Position Controller;

Based on formula (14), inverse kinematics equation can be obtained:

$\left[\begin{array}{c}{L}_1\\ L_2\end{array}\right]=J_1^{-1}\left[\begin{array}{c}{S}_y\\ y\end{array}\right]---\left(23\right)$

Adopt Position Servo:

$f={\mathrm{Kp}}_L(L_d-L)+{\mathrm{Kv}}_L({\stackrel{\&CenterDot;}{L}}_d-\stackrel{\&CenterDot;}{L})---\left(24\right)$

Wherein, f is the thrust in actuator.By adjustment controller parameter Kp _l, Kv _lthe control long to leg can be realized, and positioner can be made to have very large rigidity.But owing to often there is location estimation non-continuous event when switching leg or at irregular ground handling in Trot gait, generally just robot balance movement can be kept through driving force amplitude limit for the little deviation situation between actual value and expectation value, but Trot gait is easy to appearance actual leg length occurs and expects the widely different of leg length, at this moment by adding suitable transient process, expectation leg length can be led with regard to needing continuously.Sufficient end points kiss the earth need be ensured when adding transient process, can be as follows to the constraint condition of the long change of leg when the body angle of pitch remains unchanged:

Can be obtained by formula (7):

$m\stackrel{\&CenterDot;\&CenterDot;}{z}=f_{z1}+f_{z2}-mg>-mg---\left(25\right)$

Have again:

$\begin{array}{c}{a}_{L1}=\stackrel{\&CenterDot;\&CenterDot;}{z}/\sin \left({\mathrm{\&alpha;}}_3\right)\\ a_{L2}=\stackrel{\&CenterDot;\&CenterDot;}{z}/\sin \left({\mathrm{\&alpha;}}_4\right)\end{array}---\left(26\right)$

Therefore can be to the appropriate constraints condition of the long acceleration of leg:

$\begin{array}{c}{a}_{L1}>-g/\sin \left({\mathrm{\&alpha;}}_3\right)\\ a_{L2}>-g/\sin \left({\mathrm{\&alpha;}}_4\right)\end{array}---\left(27\right)$

Therefore, the second order transient process can adding acceleration limited to leg length is as follows:

$\begin{array}{c}{a^{\&prime;}}_{L1}={\mathrm{Kp}}_{a1}\left(L_1-L_{1r}\right)+{\mathrm{Kd}}_{a1}{\stackrel{\&CenterDot;}{L}}_{1r}\\ {a^{\&prime;}}_{L2}={\mathrm{Kp}}_{a2}\left(L_2-L_{2r}\right)+{\mathrm{Kd}}_{a2}{\stackrel{\&CenterDot;}{L}}_{2r}\end{array}---\left(28\right)$

After adding amplitude limit to acceleration, formula (29) becomes:

$\begin{array}{c}{a}_{L1}=\left\{\begin{array}{cc}{a^{\&prime;}}_{L1}& {a^{\&prime;}}_{L1}>a_{L1\_\min },a_{L1\_\min }=-k_{2}g/\sin \left({\mathrm{\&alpha;}}_3\right)\\ a_{L1\_\min }& {a^{\&prime;}}_{L1}\&le;a_{L1\_\min },a_{L1\_\min }=-k_{2}g/\sin \left({\mathrm{\&alpha;}}_3\right)\end{array}\right.\\ a_{L2}=\left\{\begin{array}{cc}{a^{\&prime;}}_{L2}& {a^{\&prime;}}_{L2}>a_{L2\_\min },a_{L2\_\min }=-k_{2}g/\sin \left({\mathrm{\&alpha;}}_4\right)\\ a_{L2\_\min }& {a^{\&prime;}}_{L2}>a_{L2\_\min },a_{L2\_\min }=-k_{2}g/\sin \left({\mathrm{\&alpha;}}_4\right)\end{array}\right.\end{array}---\left(29\right)$

Therefore leg is hoped for a long time and can be given as:

$\begin{array}{c}{L}_{1d}=L_{10}+\underset{0~t}{\&Integral;\&Integral;}{a}_{L1}d\mathrm{\&tau;}d\mathrm{\&tau;}\\ L_{2d}=L_{20}+\underset{0~t}{\&Integral;\&Integral;}{a}_{L2}d\mathrm{\&tau;}d\mathrm{\&tau;}\end{array}---\left(30\right)$

Wherein, L ₁₀, L ₂₀be respectively the initial leg of two legs long.

The overall control block diagram of planar virtual Retractive leg model can be obtained as previously mentioned, as shown in Figure 3.

3) seven-link assembly model Joint position servo control device design;

For the Controller gain variations of seven-link assembly structure and virtual Retractive leg model controller similar, power control mode need be adopted to hip joint, position servo control mode is adopted to knee joint and ankle-joint.Because single leg forward direction contains three joints, how controlled one degree of freedom relative to leg length, the object of adding three degree of freedom is mainly considered strengthen the driveability of hydraulic cylinder the driving force of single leg or improve system performance.Therefore, following carry out inverse kinematics resolve time, be simplified operation process, add joint constraint as follows:

θ ₄＝θ ₅,θ ₁＝θ ₂(31)

Be easy to get with formula (31) by shown in figure left in Fig. 2:

${\mathrm{\&theta;}}_{4d}={\mathrm{\&theta;}}_{5d}=acos\left(\frac{l_2^2+4l_1^2-L_{2d}^2}{4l_1{l}_2}\right)---\left(32\right)$

${\mathrm{\&theta;}}_{1d}={\mathrm{\&theta;}}_{2d}=acos\left(\frac{l_2^2+4l_1^2-L_{1d}^2}{4l_1{l}_2}\right)---\left(33\right)$

Servo controller is adopted to knee joint and ankle joint angle:

$\mathrm{\&tau;}={\mathrm{Kp}}_{\mathrm{\&theta;}}({\mathrm{\&theta;}}_d-\mathrm{\&theta;})+{\mathrm{Kv}}_{\mathrm{\&theta;}}({\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_d-\stackrel{\&CenterDot;}{\mathrm{\&theta;}})---\left(34\right)$

Wherein, τ is joint driven torque.By adjustment controller parameter Kp _θ, Kv _θthe tracing control to joint angles can be realized.

Below be only the preferred embodiment of the present invention, protection scope of the present invention be not only confined to above-described embodiment, all technical schemes belonged under thinking of the present invention all belong to protection scope of the present invention.It should be pointed out that for those skilled in the art, some improvements and modifications without departing from the principles of the present invention, should be considered as protection scope of the present invention.

Claims (4)

1. quadruped robot biped supports a phase place force-location mix control method, and it is characterized in that, step is:

S1: the mass motion of robot is projected on sagittal plane and Normal plane; Described sagittal plane is to support pin and perpendicular to the plane of surface level; Described Normal plane is by body barycenter and perpendicular to the plane of sagittal plane;

S2: set up Controlling model; Motion on Robot sagittal plane is reduced to plane seven-link assembly model, and the motion on Normal plane is reduced to linear inverted pendulum model; Then be planar virtual Retractive leg model by the model simplification of plane seven-link assembly; The control objectives of planar virtual Retractive leg model is height of center of mass, the body angle of pitch and horizontal shift;

S3: carry out Hybrid mode according to Controlling model; Set up the kinematical equation of planar virtual Retractive leg model and set up its kinetics equation by Newton-Euler method, position servo control method is passed through for height of center of mass and the body angle of pitch, power control mode is adopted to the horizontal shift of virtual Retractive leg areal model.

2. quadruped robot biped according to claim 1 supports phase place force-location mix control method, it is characterized in that, in step s3, body height of center of mass and the angle of pitch is controlled by leg is long, body horizontal shift is controlled, to realize controlling the Approximate Decoupling of this posture by level of control foot end power.

3. quadruped robot biped according to claim 2 supports phase place force-location mix control method, it is characterized in that, adopts classical PID controller as the outer ring controller of power, makes supporting leg foot termination touch accurately follow the tracks of expected force.

4. quadruped robot biped according to claim 2 supports phase place force-location mix control method, it is characterized in that, adopts Position Servo to control to realize the tracking to joint angles to knee joint and ankle-joint.