CN105806312A - Terrain parameter measurement method based on foot type robot with three or more supporting legs - Google Patents

Abstract

The invention discloses a terrain parameter measurement method based on a foot type robot with three or more supporting legs. The attitude angles, namely roll angle and pitch angle, of a corresponding terrain can be calculated by means of sensor information necessary for motion control of the foot type robot, the requirement for motion control of the foot type robot can be met, and the method is simple, easy to implement and high in precision. For gait control of a foot type robot according to which the number of supporting legs is larger than 2 and there are always legs in the supporting phase, the terrain attitude angles and machine body height can be calculated with the method only, an external laser radar device or vision camera is not needed, cost is reduced, complicated arithmetical operation is avoided, and precision is higher, wherein the supporting state of each leg is determined by the force of a foot end, and a leg is deemed to be in the supporting phase when the measurement value is larger than the threshold value.

Description

Legged type robot terrain parameter measuring method based on three and lower limb supported above

Technical field

The invention belongs to terrain parameter field of measuring technique, relate to a kind of legged type robot terrain parameter measuring method based on three and lower limb supported above.

Background technology

Legged type robot is mainly used in unstructured moving grids, and the walking step state in non-structured environment is different from plane environment, and the stability requirement of robot is higher, and constantly the gradient up and down of conversion is easier to be robot overturning.Legged type robot adapt to the premise of complicated non-structure environment be can terrain environment around perception, and laser radar, video image information are generally processed and draw outside terrain information by existing legged type robot, and cost is high, operand is big.

Summary of the invention

(1) goal of the invention

It is an object of the invention to: a kind of legged type robot terrain parameter measuring method based on three and lower limb supported above is provided, utilizes the information of legged type robot body sensors to be calculated, it is achieved simple and amount of calculation is little.

(2) technical scheme

In order to solve above-mentioned technical problem, the present invention provides a kind of legged type robot terrain parameter measuring method based on three and lower limb supported above, and it comprises the following steps:

Legged type robot is when walking, when there is not sliding in supporting leg, obtain the sufficient end of supporting leg of the legged type robot position vector under fuselage coordinates system according to sensor measurement, described supporting leg is the lower limb landed, and supporting leg speed is that sliding does not occur zero expression described supporting leg；

Legged type robot supporting leg position vector is write as the form of following matrix:

$P_n=\left[\begin{array}{ccc}{p}_{1\_x}& p_{1\_y}& p_{1\_z}\\ p_{2\_x}& p_{2\_y}& p_{2\_z}\\ .& .& .\\ p_{n\_x}& p_{n\_y}& p_{n\_z}\end{array}\right]---\left(1\right)$

In formula (1): p_{i_x}p_{i_y}p_{i_z}The respectively sufficient end position in i-th supporting leg xyz direction under world coordinate system, n >=3；

The normal vector of note n bar lower limb place plane is:

$\stackrel{\→}{G}=\left[\begin{array}{c}{g}_{n\_x}\\ g_{n\_y}\\ g_{n\_z}\end{array}\right]---\left(2\right)$

Then have:

Wherein,Unit vector is tieed up for n；

Solved by formula (3)Can obtain:

Unit vector be:

By greatly relative to the normal vector of world coordinate systemCan obtain greatly relative to the roll angle of world coordinate system and the angle of pitch:

In formula (6), G_rollFor greatly relative to the roll angle of world coordinate system；ForComponent on y direction；

$G_{pitch}=arctan(-\frac{{\stackrel{\→}{G}}_{n\_x}}{{\stackrel{\→}{G}}_{n\_z}})---\left(7\right)$

In formula (7), G_pitchFor the earth relative to the angle of pitch of world coordinate system,ForComponent on x direction,ForComponent on z direction；

$H_{r\_g}=-{\stackrel{\→}{G}}_n\·{\stackrel{\→}{P}}_{fi}---\left(8\right)$

In formula (8), H_{r_g}For the fuselage height relative to earth coordinates；

$H_{r\_w}=\frac{H_{r\_g}}{{\stackrel{\→}{G}}_{n\_z}}---\left(9\right)$

In formula (9), H_{r_w}For the fuselage height relative to world coordinate system.

Present invention also offers a kind of legged type robot motion control method based on upper described measuring method, utilize the attitude of landform roll angle, the angle of pitch and fuselage height adjustment legged type robot that described measuring method obtains, change with adaptation to the ground, increase the stability of robot motion.

(3) beneficial effect

The legged type robot terrain parameter measuring method based on three and lower limb supported above that technique scheme provides, the attitude angle of residing landform can be calculated just with sensor information necessary to legged type robot motor control, i.e. roll angle and the angle of pitch, can meet the requirement of legged type robot motor control, simple, precision comparison is high；For in motor process, (holding state of each lower limb is determined by the power of foot end always to have lower limb to be in support phase, measured value is more than threshold value, then think that this lower limb is in support phase), and the legged type robot gait control that supporting leg number is more than 2, landform attitude angle and fuselage height can be calculated only with this method, without external laser radar or vision camera, reducing cost, be absent from the mathematical operation of complexity, precision is higher.

Accompanying drawing explanation

Fig. 1 is the schematic diagram calculation of the measuring method of the present invention.

Detailed description of the invention

For making the purpose of the present invention, content and advantage clearly, below in conjunction with drawings and Examples, the specific embodiment of the present invention is described in further detail.

As it is shown in figure 1, the invention provides a kind of legged type robot terrain parameter measuring method, it is characterised in that comprise the following steps:

Legged type robot is when walking, when there is not sliding in supporting leg, the at least three foot end of supporting leg of the legged type robot position vector under fuselage coordinates system is obtained according to sensor measurement, described supporting leg is the lower limb landed, and supporting leg speed is that sliding does not occur zero expression described supporting leg.

The present embodiment describes for quadruped robot.As it is shown in figure 1, supporting leg is 1 lower limb, 2 lower limbs and 3 lower limbs, the position vector of each bar lower limb can pass through to be arranged on after the sensing data of leg carries out kinesiology clearing and obtain.What therefore terrain parameter was measured concretely comprises the following steps:

Legged type robot supporting leg position vector is write as the form of following matrix:

$P_n=\left[\begin{array}{ccc}{p}_{1\_x}& p_{1\_y}& p_{1\_z}\\ p_{2\_x}& p_{2\_y}& p_{2\_z}\\ .& .& .\\ p_{n\_x}& p_{n\_y}& p_{n\_z}\end{array}\right]---\left(1\right)$

In formula (1):

p_{i_x}p_{i_y}p_{i_z}The respectively sufficient end position in i-th supporting leg xyz direction under world coordinate system；

The normal vector of note n bar lower limb place plane is:

$\stackrel{\→}{G}=\left[\begin{array}{c}{g}_{n\_x}\\ g_{n\_y}\\ g_{n\_z}\end{array}\right]---\left(2\right)$

Then have:

$P_n\stackrel{\→}{G}=\stackrel{\→}{n}---\left(3\right)$

WhereinUnit vector is tieed up for n；

Solved by formula (3)Can obtain:

$\stackrel{\‾}{G}={\left({P_n}^T{P}_n\right)}^{-1}{P_n}^T\stackrel{\→}{n}---\left(4\right)$

Unit vector be:

${\stackrel{\→}{G}}_n=\frac{\stackrel{\→}{G}}{\left|\stackrel{\→}{G}\right|}---\left(5\right)$

By greatly relative to the normal vector of world coordinate systemCan obtain greatly relative to the roll angle of world coordinate system and the angle of pitch:

$G_{roll}=arcsin\left({\stackrel{\→}{G}}_{n\_y}\right)---\left(6\right)$

In formula (6), G_rollFor greatly relative to the roll angle of world coordinate system；ForComponent on y direction；

$G_{pitch}=arctan(-\frac{{\stackrel{\→}{G}}_{n\_x}}{{\stackrel{\→}{G}}_{n\_z}})---\left(7\right)$

In formula (7), G_pitchFor the earth relative to the angle of pitch of world coordinate system,ForComponent on x direction,ForComponent on z direction；

$H_{r\_g}=-{\stackrel{\→}{G}}_n\·{\stackrel{\→}{P}}_{fi}---\left(8\right)$

In formula (8), H_{r_g}For the fuselage height relative to earth coordinates；

$H_{r\_w}=\frac{H_{r\_g}}{{\stackrel{\→}{G}}_{n\_z}}---\left(9\right)$

In formula (9), H_{r_w}For the fuselage height relative to world coordinate system.

Based on above-mentioned measuring method, its application comprises the following steps: the attitude of the landform roll angle, the angle of pitch and the fuselage height adjustment legged type robot that utilize described measuring method to obtain, with the change of adaptation to the ground, increases the stability of robot motion.

Present invention can apply to the quantity of robot supporting leg and be not less than 3, such as tripodia, all as supporting leg；Or the multi-foot robots such as four-footed, five foots, Hexapod Robot.

The above is only the preferred embodiment of the present invention; it should be pointed out that, for those skilled in the art, under the premise without departing from the technology of the present invention principle; can also making some improvement and deformation, these improve and deformation also should be regarded as protection scope of the present invention.

Claims (2)

1. the legged type robot terrain parameter measuring method based on three and lower limb supported above, it is characterised in that comprise the following steps:

Legged type robot is when walking, when there is not sliding in supporting leg, obtain the sufficient end of supporting leg of the legged type robot position vector under fuselage coordinates system according to sensor measurement, described supporting leg is the lower limb landed, and supporting leg speed is that sliding does not occur zero expression described supporting leg；

Legged type robot supporting leg position vector is write as the form of following matrix:

$P_n=\left[\begin{array}{ccc}{p}_{1\_x}& p_{1\_y}& p_{1\_z}\\ p_{2\_x}& p_{2\_y}& p_{2\_z}\\ \·& \·& \·\\ p_{n\_x}& p_{n\_y}& p_{n\_z}\end{array}\right]---\left(1\right)$

In formula (1): p_{i_x}p_{i_y}p_{i_z}The respectively sufficient end position in i-th supporting leg xyz direction under world coordinate system, n >=3；

The normal vector of note n bar lower limb place plane is:

$\stackrel{\→}{G}=\left[\begin{array}{c}{g}_{n\_x}\\ g_{n\_y}\\ g_{n\_z}\end{array}\right]---\left(2\right)$

Then have:

Wherein,Unit vector is tieed up for n；

Solved by formula (3)Can obtain:

Unit vector be:

By greatly relative to the normal vector of world coordinate systemCan obtain greatly relative to the roll angle of world coordinate system and the angle of pitch:

In formula (6), G_rollFor greatly relative to the roll angle of world coordinate system；ForComponent on direction；

$G_{pitch}=arctan(-\frac{{\stackrel{\→}{G}}_{n\_x}}{{\stackrel{\→}{G}}_{n\_z}})---\left(7\right)$

In formula (7), G_pitchFor the earth relative to the angle of pitch of world coordinate system,ForComponent on direction,ForComponent on direction；

$H_{r\_g}=-{\stackrel{\→}{G}}_n\·{\stackrel{\→}{P}}_{fi}---\left(8\right)$

In formula (8), H_{r_g}For the fuselage height relative to earth coordinates；

$H_{r\_w}=\frac{H_{r\_g}}{{\stackrel{\→}{G}}_{n\_z}}---\left(9\right)$

In formula (9), H_{r_w}For the fuselage height relative to world coordinate system.

2. based on the legged type robot motion control method of measuring method described in claim 1, it is characterized in that, utilize the attitude of landform roll angle, the angle of pitch and fuselage height adjustment legged type robot that described measuring method obtains, change with adaptation to the ground, increase the stability of robot motion.