CN103042526A - Method and device for controlling to support foot of humanoid robot in single leg supporting period - Google Patents

Abstract

The invention discloses a method and a device for controlling to support a foot of a humanoid robot in a single leg supporting period and belongs to the field of robots. An expected virtual zero moment point is arranged outside the foot supporting region, and a step increasing quantity generated through the rotation of the robot can be acquired. The method comprises steps of calculating the virtual zero moment point and detecting the rotation angle of the supporting foot; determining the rotation angle of the supporting foot; determining the position of the virtual zero moment point; and limiting the position of the virtual zero moment point. The device comprises a calculating and detecting module, a rotary angle control module, a virtual zero moment point position control module and a virtual zero moment point position limiting module. By the aid of the method and the device, the humanoid robot can support the foot to rotate in the single leg supporting period, the walking step is increased, and the control is simple and reliable.

Description

Anthropomorphic robot is controlled the method and apparatus of single pin support phase support sole

Technical field:

The present invention relates to a kind of anthropomorphic robot and control the method and apparatus that the rotation of single pin support phase support sole increases stride, belong to the Robotics field.

Background technology:

The raising of the speed of travel is focus and the difficult point of anthropomorphic robot area research always.The raising of the speed of travel can be by increasing stride and improving the walking frequency and realize.The raising of walking frequency is had relatively high expectations to the rigidity of anthropomorphic robot frame for movement, the driving force of driving element, and is stronger for composite request and the dependence of related discipline.At present, the increase of walking stride mainly realizes by increasing Leg length.

The walking step state method for designing of anthropomorphic robot usually point of zero moment is set in the contact area (supporting zone) on feet and ground with guarantee anthropomorphic robot in the process of walking feet contact fully with ground, do not rotate.

The definition of point of zero moment is that the suitable reacting force of supporting that the plantar pressure of supposition anthropomorphic robot supporting leg can be applied in the vola any represents, on in ground contact surface this aspect, all active forces that robot is subject to and the component of the resultant moment of moment in horizontal plane are zero so.Be that point of zero moment is the central point of making a concerted effort that foot contacts suffered active force in the biped walking process with ground.If the point of zero moment of robot drops on feet and contacts with ground in the convex polygon supporting zone that forms, the component of anthropomorphic robot rotating close moment in horizontal plane is zero, anthropomorphic robot can not rotate in the biped walking process in theory, otherwise will make the robot generation around the rotation of horizontal plane, as shown in Figure 1.

But human single pin support phase in fast walking has the process of feet rotation.According to the related law of human body walking, the speed of travel is higher, and it is higher that single pin supports the phase proportion.Single pin support phase shared time scale in human walking process is high, forward travel distance is large, and it is larger on human speed of travel impact that therefore single pin supports the phase.The feet rotation can increase stride and single pin supports the forward travel distance of phase, thereby improves the speed of travel.

Instructed a kind of biped robot's traveling method with feet rotatable phase in the prior art.In the method, anthropomorphic robot has rotatable phase in single pin support phase.But, the method is by foot being rotated indication point (Foot Rotation Indicator, FRI) be strict controlled in that feet realizes previously, the method does not illustrate the relation of rotation degree and foot rotation indication point (FRI) position and step change, and implementation method is comparatively complicated.

Instructed a kind of track optimizing method of feet rotation in the prior art.In the method, anthropomorphic robot has rotatable phase in single pin support phase, and the point of zero moment of anthropomorphic robot is controlled in the tiptoe place.The method has only studied how to save the targets such as energy by feet rotation realization, does not relate to how making feet rotate the step-length recruitment that obtains to expect outward by point of zero moment being set in supporting zone.

After prior art being investigated and explore, the inventor has proposed a kind of anthropomorphic robot and has controlled the method and apparatus that the rotation of single pin support phase support sole increases the walking stride.From in the existing method point of zero moment is set in the supporting zone with prevent robot produce rotation different be, the present invention is set in point of zero moment outside the supporting zone, the rotation that makes robot produce expectation increases stride, and this point of zero moment that exceeds supporting zone can be called the fictitious zero Moment Point.Point of zero moment is identical with the computational methods of fictitious zero Moment Point, just is set at outside the supporting zone.The present invention makes the anthropomorphic robot list pin support phase realize adjustable rotation by the fictitious zero Moment Point is set in outside the supporting zone, can increase Humanoid Robot Based on Walking stride and speed, and method is simple, and is feasible.

Summary of the invention:

For problems of the prior art, the object of the present invention is to provide a kind of anthropomorphic robot to control single pin support phase and support the method and apparatus that the sole rotation increases stride, can simply, effectively improve Humanoid Robot Based on Walking speed.

The technical solution used in the present invention is as follows:

A kind of anthropomorphic robot is controlled single pin support phase and is supported the method that the sole rotation increases stride, comprising:

Calculate the fictitious zero Moment Point and detect the step that supports the sole anglec of rotation;

Determine to support the step of the sole anglec of rotation;

Determine the step of fictitious zero Moment Point position;

The step of restriction fictitious zero Moment Point position;

According to above-mentioned steps, by calculating the formula of fictitious zero Moment Point, set the fictitious zero Moment Point with respect to the supporting zone border apart from d, make anthropomorphic robot at the θ that rotates a certain angle of single pin support phase, thereby increase stride Δ D and the speed of walking, be conducive to the fast realization of walking.

Preferably, calculate in the step of fictitious zero Moment Point and the detection support sole anglec of rotation, described fictitious zero Moment Point is identical with described point of zero moment computational methods, comprising:

In each control cycle, calculate fictitious zero Moment Point position:

$x_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\Σ}}{m}_i({\stackrel{\·\·}{z}}_i+g)x_i-\underset{i}{\mathrm{\Σ}}{m}_i{\stackrel{\·\·}{x}}_i{z}_i-\underset{i}{\mathrm{\Σ}}{I}_{\mathrm{iy}}{\stackrel{\·\·}{\mathrm{\Ω}}}_{\mathrm{iy}}}{\underset{i}{\mathrm{\Σ}}{m}_i({\stackrel{\·\·}{z}}_i+g)}$

$y_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\Σ}}{m}_i({\stackrel{\·\·}{z}}_i+g)y_i-\underset{i}{\mathrm{\Σ}}{m}_i{\stackrel{\·\·}{y}}_i{z}_i-\underset{i}{\mathrm{\Σ}}{I}_{\mathrm{ix}}{\stackrel{\·\·}{\mathrm{\Ω}}}_{\mathrm{ix}}}{\underset{i}{\mathrm{\Σ}}{m}_i({\stackrel{\·\·}{z}}_i+g)}$

M wherein _iBe the quality of i part, g is acceleration of gravity, I _IxAnd I _IyRespectively that anthropomorphic robot i part is along the rotary inertia of X-axis and Y-axis, Ω _IxAnd Ω _IyThat the i part center of gravity of robot is around the absolute angle displacement of X-axis and Y-axis; (x _i, y _i, z _i) be the absolute coordinate of barycenter in coordinate system of the i of robot part, x _Zmp, y _ZmpBe the position coordinates of fictitious zero Moment Point in X-axis and Y-axis;

In each control cycle, utilize the angle detection device that is installed on the sole to detect and support sole anglec of rotation θ.

Preferably, the relation of step-length recruitment and the anglec of rotation is determined by following formula in the step of described definite support sole anglec of rotation:

$\mathrm{\&Delta;D}=x=1_7+\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7})$

Following formula can briefly be described as: Δ D=f ₁(θ), f ₁It is functional relation;

Above h is the height of center of mass data that walking is set, l ₇For ankle on the sole to tiptoe is in distance on the directions X, be known quantity.

Preferably, feet anglec of rotation θ and fictitious zero Moment Point are calculated by following formula apart from the relation apart from d on supporting zone border in the step of described definite fictitious zero Moment Point position:

$1_7+d=p_x=x-\frac{(h+1_6)\stackrel{\&CenterDot;\&CenterDot;}{x}}{g}$

$x=1_7+\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7})$

Feet anglec of rotation θ and fictitious zero Moment Point can be by following formula schematic representation apart from the relation of d apart from the supporting zone border:

θ=f ₂(d), f ₂It is functional relation.

Preferably, the fictitious zero Moment Point is calculated by following formula to supporting border ultimate range n in the step of described restriction fictitious zero Moment Point position:

n＝L-l

$L=\underset{i=1}{\overset{6}{\mathrm{\&Sigma;}}}{1}_i\&CenterDot;\cos {\mathrm{\&theta;}}_i$

l＝1 ₇·cosθ ₇＝1 ₇·cosθ _max

Wherein, length is 1 between apery robot ankle joint and the sole ₁, itself and ground angle theta ₁Be fixed value 90 degree, similarly, other lower limb partial-lengths are 1 _i, the corresponding ground angle is θ _i(i=2,3 ..., 7), wherein, θ ₇=θ _Max, i.e. the maximum anglec of rotation of feet;

Therefore, stride recruitment and the feet anglec of rotation and fictitious zero Moment Point are expressed as apart from the distance relation on supporting zone border:

Δ D=f ₁(θ)=f ₁(f ₂And d≤n (d)).

A kind of anthropomorphic robot list pin support phase is supported the control system that the sole rotation increases stride, described control system is by calculating the formula of fictitious zero Moment Point, set the fictitious zero Moment Point with respect to the supporting zone border apart from d, make anthropomorphic robot at the θ that rotates a certain angle of single pin support phase, thereby increase stride Δ D and the speed of walking, be conducive to the fast realization of walking, it is characterized in that described control system comprises:

Be used for calculating the fictitious zero Moment Point and detect the device that supports the sole anglec of rotation, it calculates fictitious zero Moment Point position and detects the feet sole anglec of rotation;

Be used for control and support the device of the sole anglec of rotation, its stride recruitment according to expectation determines that single pin supports the phase feet sole anglec of rotation;

Be used for the device of control fictitious zero Moment Point position, its support sole anglec of rotation according to expectation is determined fictitious zero Moment Point position;

The device that is used for restriction fictitious zero Moment Point position, its restriction fictitious zero Moment Point exceeds the ultimate range on supporting zone border.

Preferably, described device for calculating fictitious zero Moment Point and the detection support sole anglec of rotation comprises:

Be used for calculating in each control cycle the device of fictitious zero Moment Point position, computing formula is as follows:

$x_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)x_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{x}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{iy}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{iy}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

$y_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)y_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{y}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{ix}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{ix}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

M wherein _iBe the quality of i part, g is acceleration of gravity, I _IxAnd I _IyRespectively that anthropomorphic robot i part is along the rotary inertia of X-axis and Y-axis, Ω _IxAnd Ω _IyThat the i part center of gravity of robot is around the absolute angle displacement of X-axis and Y-axis; (x _i, y _i, z _i) be the absolute coordinate of barycenter in coordinate system of the i of robot part, x _Zmp, y _ZmpBe the position coordinates of fictitious zero Moment Point in X-axis and Y-axis.

Be used in each control cycle, detecting the device that supports sole anglec of rotation θ with the angle detection device that is installed on the sole.

Preferably, the described device step-length recruitment that supports the sole anglec of rotation for control is definite by following formula with the relation of the anglec of rotation:

$\mathrm{\&Delta;D}=x=1_7+\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7})$

Wherein, h is the height of center of mass data that walking is set, l ₇For ankle on the sole to tiptoe is in distance on the x direction, be known quantity.

Following formula can briefly be described as: Δ D=f ₁(θ), f ₁It is functional relation.

Preferably, the device feet anglec of rotation θ and the fictitious zero Moment Point that are used for control fictitious zero Moment Point position are calculated by following formula apart from the relation apart from d on supporting zone border:

$1_7+d=p_x=x-\frac{(h+1_6)\stackrel{\&CenterDot;\&CenterDot;}{x}}{g}$

$x=1_7+\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7})$

Feet anglec of rotation θ and fictitious zero Moment Point can be by following formula schematic representation apart from the relation of d apart from the supporting zone border:

θ=f ₂(d), f ₂It is functional relation.

Preferably, the device fictitious zero Moment Point that is used for restriction fictitious zero Moment Point position is calculated by following formula to supporting border ultimate range n:

n＝L-l

$L=\underset{i=1}{\overset{6}{\mathrm{\&Sigma;}}}{1}_i\&CenterDot;\cos {\mathrm{\&theta;}}_i$

l＝1 ₇·cosθ ₇＝1 ₇·cosθ _max

Wherein, length is 1 between apery robot ankle joint and the sole ₁, itself and ground angle theta ₁Be fixed value 90 degree, similarly, other lower limb partial-lengths are 1 _i, the corresponding ground angle is θ _i(i=2,3 ..., 7), wherein, θ ₇=θ _Max, i.e. the maximum anglec of rotation of feet;

Therefore, stride recruitment and the feet anglec of rotation and fictitious zero Moment Point are expressed as apart from the distance relation on supporting zone border:

Δ D=f ₁(θ)=f ₁(f ₂And d≤n (d)).

The invention has the beneficial effects as follows:

When 1, using for reference human fast walking, there is the principle of rotation in single pin support phase, makes anthropomorphic robot produce rotation in single pin support phase and realizes the increase of stride and the raising of pace.

2, the distance that exceeds single pin supporting zone by setting the fictitious zero Moment Point, the realization anthropomorphic robot supports sole rotation in various degree, and then obtains the step-length recruitment of expectation, and control method is simple, effective.

Description of drawings:

Fig. 1 anthropomorphic robot point of zero moment and supporting zone concern schematic diagram;

Fig. 2 fictitious zero Moment Point position and the feet anglec of rotation and step-length increase the relationship between quantities schematic diagram;

Fig. 3 fictitious zero Moment Point exceeds the ultimate range schematic diagram of supporting zone;

Fig. 4 controls the feet rotation and increases the stride flow chart.

The specific embodiment:

For making purpose of the present invention, technical scheme clearer, the below will take anthropomorphic robot fast walking describe in detail further embodiment of the present invention as example.

A kind of anthropomorphic robot is controlled single pin support phase and is supported the method that the sole rotation increases stride, may further comprise the steps:

Calculate the fictitious zero Moment Point and detect the support sole anglec of rotation;

Supporting the sole anglec of rotation determines;

Fictitious zero Moment Point location positioning;

Fictitious zero Moment Point position limitation

The coordinate system (Fig. 1) that model the present invention is used.Coordinate origin is positioned at the ankle place of feet, and direction of advance is+X, straight up direction be+the Z(+Y direction determined by right-hand rule).During robot ambulation, the barycenter of anthropomorphic robot (can be equal to the waist joint mid point) is set at and supports directly over the ankle to guarantee that body posture is vertical.The center-of-mass coordinate of anthropomorphic robot is on direction of advance at this moment, and namely the directions X initial value is 0.That is to say that when the normal walking of robot does not rotate the robot center-of-mass coordinate is that (0, h), wherein, h is the height of center of mass data that walking is set, and is known quantity.When rotation occured, barycenter was that the waist joint mid point rotates around the tiptoe place, and its location variation on directions X is the center-of-mass coordinate value.

Described step " is calculated the fictitious zero Moment Point and is detected the support sole anglec of rotation " and comprising:

The fictitious zero Moment Point is identical with the point of zero moment computational methods.

Calculate fictitious zero Moment Point position in each control cycle, computing formula is as follows:

$x_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)x_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{x}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{iy}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{iy}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

$y_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)y_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{y}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{ix}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{ix}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

Definition m _iBe the quality of i part, g is acceleration of gravity, I _IxAnd I _IyRespectively that anthropomorphic robot i part is along the rotary inertia of X-axis and Y-axis, Ω _IxAnd Ω _IyThat the i part center of gravity of robot is around the absolute angle displacement of X-axis and Y-axis; (x _i, y _i, z _i) be the absolute coordinate of barycenter in coordinate system of the i of robot part, x _Zmp, y _ZmpBe the position coordinates of fictitious zero Moment Point in X-axis and Y-axis.

Utilize the angle detection device that is installed on the sole to detect in each control cycle and support sole anglec of rotation θ.

Described step " supporting the sole anglec of rotation determines " comprising:

In the Humanoid Robot Based on Walking process, can control feet whole health of when rotation around the rotation of tiptoe place, each joint relative position of supporting leg is constant, and leading leg, it is low being parallel before guaranteeing to occur with respect to rotation to raise or lift in direction of advance.When rotation occurs, the tiptoe place be pivot to the distance of waist joint mid point be radius of turn C, as shown in Figure 2:

$C=\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}$

${\mathrm{\&theta;}}_1=\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7}$

Support sole around the anglec of rotation θ at tiptoe place, satisfy following relation:

θ ₂＝π-θ-θ ₁

Obtain thus, behind the feet rotation expected angle θ, point coordinates is in barycenter or the waist joint:

x＝1 ₇+C·cosθ ₂

Because lead leg parallel only occurs on direction of advance, so step-length recruitment Δ D is that the displacement of barycenter on directions X equates with the waist joint mid point, thereby obtained the relation of step-length recruitment and the anglec of rotation, can be determined by following formula:

$\mathrm{\&Delta;D}=x=1_7+\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7})$

Wherein, h is the height of center of mass data that walking is set, l ₇For ankle on the sole to tiptoe is in distance on the directions X, be known quantity.

Following formula can briefly be described as: Δ D=f ₁(θ), f ₁It is functional relation.

Described step " fictitious zero Moment Point location positioning " comprising:

When the point of zero moment position of anthropomorphic robot was in supporting zone, the component of anthropomorphic robot rotating close moment in horizontal plane was zero, and anthropomorphic robot can not rotate in the biped walking process.Be called the fictitious zero Moment Point in the time of outside point of zero moment exceeds supporting zone, the component of rotating close moment in horizontal plane is non-vanishing, and anthropomorphic robot can produce rotation.

The point of zero moment formula of being released by inverted pendulum model can be used for calculating the fictitious zero Moment Point, and formula is as follows:

Wherein, p _x, p _zBe respectively the fictitious zero Moment Point at X, the coordinate on the Z direction, x and z are respectively that barycenter is point coordinates in the waist joint, g is acceleration of gravity.

During robot ambulation, because the anglec of rotation and little, the anthropomorphic robot barycenter is that the variation of waist joint mid point in the vertical direction is less, can think waist joint on level altitude, and namely h is constant.When robot walks in the level ground, p _z=-1 ₆, then fictitious zero Moment Point formula can be reduced to:

$p_x=x-\frac{(h+1_6)\stackrel{\&CenterDot;\&CenterDot;}{x}}{g}$

Feet anglec of rotation θ and fictitious zero Moment Point can be calculated by following formula apart from the relation apart from d on supporting zone border, as shown in Figure 2:

$1_7+d=p_x=x-\frac{(h+1_6)\stackrel{\&CenterDot;\&CenterDot;}{x}}{g}$

$x=1_7+\sqrt{{\mathrm{<figure-callout\; id="2"\; label="h"\; filenames="HDA00002759376100021.png"\; state="\{\{state\}\}">h</figure-callout>}}^2+{1_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\mathrm{<figure-callout\; id="1"\; label="arctan\; h"\; filenames="HDA00002759376100012.png,HDA00002759376100021.png"\; state="\{\{state\}\}">arctan</figure-callout>}\frac{h}{}\frac{}{1_7})$

Feet anglec of rotation θ and fictitious zero Moment Point can be by following formula schematic representation apart from the relation of d apart from the supporting zone border:

θ=f ₂(d), f ₂It is the functional relation that above-mentioned formula obtains.

Described step " fictitious zero Moment Point position limitation " comprising:

When if the fictitious zero Moment Point surpasses certain value apart from the supporting zone border (being made as n herein), the anthropomorphic robot fast rotational, making leads leg contacts to earth too early, causes the robot disequilibrium.Therefore, in control when rotation anthropomorphic robot list pin support phase,, fictitious zero Moment Point position should be no more than n, as shown in Figure 3.The length of anthropomorphic robot lower limb each several part is known, and the angle of each several part and horizontal plane can obtain by the joint position sensor.Among the figure, length is 1 between ankle arthrosis and the sole ₁, itself and ground angle theta ₁Be fixed value 90 degree.Similarly, other lower limb partial-lengths are 1 _i, the corresponding ground angle is θ _i(i=2,3 ..., 7), wherein, θ ₇=θ _Max, i.e. the maximum anglec of rotation of feet.The fictitious zero Moment Point can be calculated by following formula to supporting border ultimate range n:

n＝L-l

$L=\underset{i=1}{\overset{6}{\mathrm{\&Sigma;}}}{1}_i\&CenterDot;\cos {\mathrm{\&theta;}}_i$

l＝1 ₇·cosθ ₇＝1 ₇·cosθ _max

Therefore, stride recruitment and the feet anglec of rotation and fictitious zero Moment Point should be expressed as apart from the distance relation on supporting zone border:

Δ D=f ₁(θ)=f ₁(f ₂And d≤n (d)).

According to above-mentioned steps, by calculating the formula of fictitious zero Moment Point, set the fictitious zero Moment Point with respect to the supporting zone border apart from d, can make anthropomorphic robot at the θ that rotates a certain angle of single pin support phase, thereby increase stride Δ D and the speed of walking, be conducive to the fast realization of walking, its flow process as shown in Figure 4.

With reference to flow process shown in Figure 4, can exceed by setting the fictitious zero Moment Point distance on supporting zone border, obtain the step-length recruitment of expectation, and then improve robot ambulation speed.

The embodiment of the invention exceeds the distance on supporting zone border by setting the fictitious zero Moment Point, make anthropomorphic robot produce rotation in single pin support phase, increases the step-length of expectation, is conducive to improve the speed of travel.

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

Claims (10)

1. an anthropomorphic robot is controlled single pin support phase and is supported the method that the sole rotation increases stride, comprising:

Calculate the fictitious zero Moment Point and detect the step that supports the sole anglec of rotation;

Determine to support the step of the sole anglec of rotation;

Determine the step of fictitious zero Moment Point position;

The step of restriction fictitious zero Moment Point position;

According to above-mentioned steps, by calculating the formula of fictitious zero Moment Point, set the fictitious zero Moment Point with respect to the supporting zone border apart from d, make anthropomorphic robot at the θ that rotates a certain angle of single pin support phase, thereby increase stride Δ D and the speed of walking, be conducive to the fast realization of walking.

2. method according to claim 1 is wherein calculated in the step of fictitious zero Moment Point and the detection support sole anglec of rotation, and described fictitious zero Moment Point is identical with described point of zero moment computational methods, comprising:

In each control cycle, calculate fictitious zero Moment Point position:

$x_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)x_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{x}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{iy}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{iy}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

$y_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)y_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{y}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{ix}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{ix}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

M wherein _iBe the quality of i part, g is acceleration of gravity, I _IxAnd I _IyRespectively that anthropomorphic robot i part is along the rotary inertia of X-axis and Y-axis, Ω _IxAnd Ω _IyThat the i part center of gravity of robot is around the absolute angle displacement of X-axis and Y-axis; (x _i, y _i, z _i) be the absolute coordinate of barycenter in coordinate system of the i of robot part, x _Zmp, y _ZmpBe the position coordinates of fictitious zero Moment Point in X-axis and Y-axis;

In each control cycle, utilize the angle detection device that is installed on the sole to detect and support sole anglec of rotation θ.

3. method according to claim 2, the relation of step-length recruitment and the anglec of rotation is determined by following formula in the step of wherein said definite support sole anglec of rotation:

$\mathrm{\&Delta;D}=x=l_7+\sqrt{h^2+{l_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\arctan \frac{h}{l_7})$

Following formula can briefly be described as: Δ D=f ₁(θ), f ₁It is functional relation;

Above h is the height of center of mass data that walking is set, l ₇For ankle on the sole to tiptoe is in distance on the directions X, be known quantity.

4. method according to claim 3, feet anglec of rotation θ and fictitious zero Moment Point are calculated by following formula apart from the relation apart from d on supporting zone border in the step of wherein said definite fictitious zero Moment Point position:

$l_7+d=p_x=x-\frac{(h+l_6)\stackrel{\&CenterDot;\&CenterDot;}{x}}{g}$

$x=l_7+\sqrt{h^2+{l_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\arctan \frac{h}{l_7})$

Feet anglec of rotation θ and fictitious zero Moment Point can be by following formula schematic representation apart from the relation of d apart from the supporting zone border:

θ=f ₂(d), f ₂It is functional relation.

5. method according to claim 4, the fictitious zero Moment Point is calculated by following formula to supporting border ultimate range n in the step of wherein said restriction fictitious zero Moment Point position:

n＝L-l

$L=\underset{i=1}{\overset{6}{\mathrm{\&Sigma;}}}{l}_i\&CenterDot;\cos {\mathrm{\&theta;}}_i$

l＝l ₇·cosθ ₇＝l ₇·cosθ _max

Wherein, length is l between apery robot ankle joint and the sole ₁, itself and ground angle theta ₁Be fixed value 90 degree, similarly, other lower limb partial-lengths are l _i, the corresponding ground angle is θ _i(i=2,3 ..., 7), wherein, θ ₇=θ _Max, i.e. the maximum anglec of rotation of feet;

Therefore, stride recruitment and the feet anglec of rotation and fictitious zero Moment Point are expressed as apart from the distance relation on supporting zone border:

Δ D=f ₁(θ)=f ₁(f ₂And d≤n (d)).

6. an anthropomorphic robot list pin support phase is supported the control system that the sole rotation increases stride, described control system is by calculating the formula of fictitious zero Moment Point, set the fictitious zero Moment Point with respect to the supporting zone border apart from d, make anthropomorphic robot at the θ that rotates a certain angle of single pin support phase, thereby increase stride Δ D and the speed of walking, be conducive to the fast realization of walking, it is characterized in that described control system comprises:

Be used for calculating the fictitious zero Moment Point and detect the device that supports the sole anglec of rotation, it calculates fictitious zero Moment Point position and detects the feet sole anglec of rotation;

Be used for control and support the device of the sole anglec of rotation, its stride recruitment according to expectation determines that single pin supports the phase feet sole anglec of rotation;

Be used for the device of control fictitious zero Moment Point position, its support sole anglec of rotation according to expectation is determined fictitious zero Moment Point position;

The device that is used for restriction fictitious zero Moment Point position, its restriction fictitious zero Moment Point exceeds the ultimate range on supporting zone border.

7. control system according to claim 6 wherein saidly comprises for calculating the fictitious zero Moment Point and detecting the device that supports the sole anglec of rotation:

Be used for calculating in each control cycle the device of fictitious zero Moment Point position, computing formula is as follows:

$x_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)x_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{x}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{iy}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{iy}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

$y_{\mathrm{ZMP}}=\frac{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)y_i-\underset{i}{\mathrm{\&Sigma;}}{m}_i{\stackrel{\&CenterDot;\&CenterDot;}{y}}_i{z}_i-\underset{i}{\mathrm{\&Sigma;}}{I}_{\mathrm{ix}}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Omega;}}}_{\mathrm{ix}}}{\underset{i}{\mathrm{\&Sigma;}}{m}_i({\stackrel{\&CenterDot;\&CenterDot;}{z}}_i+g)}$

M wherein _iBe the quality of i part, g is acceleration of gravity, I _IxAnd I _IyRespectively that anthropomorphic robot i part is along the rotary inertia of X-axis and Y-axis, Ω _iX and Ω _IyThat the i part center of gravity of robot is around the absolute angle displacement of X-axis and Y-axis; (x _i, y _i, z _i) be the absolute coordinate of barycenter in coordinate system of the i of robot part, x _Zmp, y _ZmpBe the position coordinates of fictitious zero Moment Point in X-axis and Y-axis.

Be used in each control cycle, detecting the device that supports sole anglec of rotation θ with the angle detection device that is installed on the sole.

8. control system according to claim 7, the wherein said device step-length recruitment that supports the sole anglec of rotation for control is definite by following formula with the relation of the anglec of rotation:

$\mathrm{\&Delta;D}=x=l_7+\sqrt{h^2+{l_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\arctan \frac{h}{l_7})$

Wherein, h is the height of center of mass data that walking is set, l ₇For ankle on the sole to tiptoe is in distance on the x direction, be known quantity.

Following formula can briefly be described as: Δ D=f ₁(θ), f ₁It is functional relation.

9. control system according to claim 8, the device feet anglec of rotation θ and the fictitious zero Moment Point that wherein are used for control fictitious zero Moment Point position are calculated by following formula apart from the relation apart from d on supporting zone border:

$l_7+d=p_x=x-\frac{(h+l_6)\stackrel{\&CenterDot;\&CenterDot;}{x}}{g}$

$x=l_7+\sqrt{h^2+{l_7}^2}\&CenterDot;\cos (\mathrm{\&pi;}-\mathrm{\&theta;}-\arctan \frac{h}{l_7})$

Feet anglec of rotation θ and fictitious zero Moment Point can be by following formula schematic representation apart from the relation of d apart from the supporting zone border:

θ=f ₂(d), f ₂It is functional relation.

10. control system according to claim 9, the device fictitious zero Moment Point that wherein is used for restriction fictitious zero Moment Point position is calculated by following formula to supporting border ultimate range n:

n＝L-l

$L=\underset{i=1}{\overset{6}{\mathrm{\&Sigma;}}}{l}_i\&CenterDot;\cos {\mathrm{\&theta;}}_i$

l＝l ₇·cosθ ₇＝l ₇·cosθ _max

Wherein, length is l between apery robot ankle joint and the sole ₁, itself and ground angle theta ₁Be fixed value 90 degree, similarly, other lower limb partial-lengths are l _i, the corresponding ground angle is θ _i(i=2,3 ..., 7), wherein, θ ₇=θ _Max, i.e. the maximum anglec of rotation of feet;

Therefore, stride recruitment and the feet anglec of rotation and fictitious zero Moment Point are expressed as apart from the distance relation on supporting zone border:

Δ D=f ₁(θ)=f ₁(f ₂And d≤n (d)).

Images