CN101403619A - Foot gesture real-time detection system and method for human-imitated robot - Google Patents

Abstract

The invention discloses a humanoid robot foot posture real-time detection system and a method, which belongs to the automation field. The system comprises: a DSP controller, a three-axis acceleration sensor and an angular rate gyroscope; the DSP controller gains a piece of static foot surface inclining information through the three-axis acceleration sensor, and gains an angle variety value through the angular rate speed gyroscope, and obtains the real-time foot surface inclining information according to the static foot surface inclining information and the angle variety value. The invention also discloses a humanoid robot foot posture real-time detection method, which comprises the steps of: obtaining the static foot surface inclining information by calculating the collected three-axis acceleration information; obtaining the angle variety value by calculating the collected angle speed information; and obtaining the real-time foot surface inclining information by calculating the static foot surface inclining information and the angle variety value. The humanoid robot foot posture real-time detection system and method provided by the invention can real-time and continuously measure the foot surface inclining angle.

Description

Humanoid robot foot posture real-time detection and method

Technical field

The present invention relates to automatic field, particularly a kind of humanoid robot foot posture real-time detection and method.

Background technology

Anthropomorphic robot is and the immediate a kind of robot of the mankind, it is compared with traditional wheeled robot or caterpillar robot, more can adapt to human daily life environment, is more convenient for using the various tool of inventing design as the mankind simultaneously, therefore, anthropomorphic robot has vast potential for future development.

At present, research for anthropomorphic robot still mainly concentrates on effective more, the reliable stability control method of exploration, in these researchs, the foot of robot just becomes the key point that solves apery robot stabilized walking as contacting and stressed position with the external world is unique in the walking process.Under the environment of some more complicated, during such as walking on roughness pavement, turning, stair activity, its foot gesture information just seems even more important.And the foot surface inclining angle information in the foot gesture information is the important evidence of apery robot stabilized control.It can allow anthropomorphic robot perception instep inclination situation, and when settling touch-down, foot surface inclining angle can also reflect the terrain slope state, is a key parameter of apery robot stabilized control.

At present, to the detection of humanoid robot foot section attitude, a kind of method for designing of foot of anthropomorphic robot is arranged in the prior art, can obtain foot gesture foot surface inclining angle information by this method.Referring to Fig. 1, this foot of anthropomorphic robot comprises Rubber foot bottom, sole, six-dimension force sensor, upper flange, lower flange, obliquity sensor, tactile array pad, signal processing system and instep.Signal processing system is installed in the front portion of sole, and obliquity sensor is installed in the middle part, and six-dimension force sensor is installed in the heel place.When robot ambulation, the ground reaction force when pin contacts with the road surface in the process is walked by the six-dimension force sensor detection machine People's Bank of China on the foot of anthropomorphic robot, and the angle of inclination that obliquity sensor detects sole is a foot gesture foot surface inclining angle information.

After above-mentioned prior art was analyzed, the inventor found:

These obliquity sensors of available technology adopting can only detect the inclination angle under the static condition, promptly robot foot must with the foot surface inclining angle under the stable condition that contacts in ground.But, in anthropomorphic robot real work engineering, it will finish multiple actions such as walking, turning, stair activity, nautch, there is relative motion between most time pin and the ground, rather than be in static state, so the method for this detection anthropomorphic robot foot surface inclining angle has significant limitation.

Summary of the invention

In order to solve the limitation that to measure the anthropomorphic robot foot surface inclining angle under the static condition in the prior art, the present invention proposes a kind of humanoid robot foot posture real-time detection and method.

Described technical scheme is as follows:

A kind of humanoid robot foot posture real-time detection, described system comprises:

Dsp controller, 3-axis acceleration sensor, angular rate gyroscope;

Described dsp controller is used for to described 3-axis acceleration sensor write control signal, and the output of described 3-axis acceleration sensor is calculated static foot surface inclining angle information;

Described dsp controller is used for to described angular rate gyroscope write control signal, and the output of described angular rate gyroscope is calculated the angle changing value;

Described dsp controller calculates according to described static foot surface inclining angle information and angle changing value, obtains real-time foot surface inclining angle information.

Described dsp controller specifically comprises:

First read module is used for reading 3-axis acceleration information from described 3-axis acceleration sensor;

First computing module: be used for described 3-axis acceleration information is calculated static foot surface inclining angle information, specifically be used for:

If Ax is the accekeration of x axle acceleration sensor output, Ay is the accekeration of y axle acceleration sensor output, and Az is the accekeration of z axle acceleration sensor output; θ x is x axle and horizontal plane angle, and θ y is y axle and horizontal plane angle, and θ z is z axle and horizontal plane angle;

Described accekeration Ax is revolved computing anyway obtain described θ x;

Described accekeration Ay is revolved computing anyway obtain described θ y;

Described accekeration Az is revolved computing anyway obtain described θ z;

Promptly (Ax, Ay Az) have obtained described static foot surface inclining angle (θ x, θ y, θ z) according to described 3-axis acceleration information.

Described angular rate gyroscope comprises x shaft angle rate gyroscope, y shaft angle rate gyroscope and z shaft angle rate gyroscope; Described dsp controller specifically comprises:

Second read module is used for reading angular speed information from described angular rate gyroscope;

Second computing module is used for described angular speed information is calculated the angle changing value, specifically is used for:

If r _x(i) be the t=iT angular speed information of x shaft angle rate gyroscope output constantly, r _y(i) be the t=iT angular speed information of y shaft angle rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _XcBe moment x shaft angle degree changing value from t=0 to t=iT, θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To described x shaft angle rate information r _x(i) carry out integration and obtain described angle changing value θ _Xc

To described y shaft angle rate information r _y(i) carry out integration and obtain described angle changing value θ _Yc

To described z shaft angle rate information r _c(i) carry out integration and obtain described angle changing value θ _Zc

Promptly according to described angular speed information (r _x(i), r _y(i), r _z(i)) obtained described angle changing value (θ _Xc, θ _Yc, θ _Zc).

Described dsp controller also comprises summation module, is used for calculating according to described static foot surface inclining angle information and angle changing value, obtains real-time foot surface inclining angle information, specifically is used for:

If θ _XtBe the real-time inclination angle between x axle and the surface level, θ _YtBe the real-time foot surface inclining angle between y axle and the surface level, θ _ZtBe the real-time foot surface inclining angle between z axle and the surface level;

θ _X0Be the static foot surface inclining angle at t=0 moment x axle, θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To described θ _X0With described θ _XcSummation obtains the real-time foot surface inclining angle θ of x axle _Xt

To described θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt

To described θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt

Described real-time foot surface inclining angle information is (θ _Xt, θ _Yt, θ _Zt).

Described angular speed information is x shaft angle rate information, y shaft angle rate information and z shaft angle rate information.

A kind of humanoid robot foot section attitude real-time detection method, described method comprises:

The 3-axis acceleration information of gathering is calculated static foot surface inclining angle information;

The angular speed information of gathering is calculated the angle changing value;

Calculate according to described static foot surface inclining angle information and described angle changing value, obtain real-time foot surface inclining angle information.

Described 3-axis acceleration information to collection calculates static foot surface inclining angle information, specifically comprises:

If Ax is the accekeration of x axle acceleration sensor output, Ay is the accekeration of y axle acceleration sensor output, and unit is g, and Az is the accekeration of z axle acceleration sensor output; θ x is x axle and horizontal plane angle, and θ y is y axle and horizontal plane angle, and θ z is z axle and horizontal plane angle;

Described accekeration Ax is revolved computing anyway obtain described θ x;

Described accekeration Ay is revolved computing anyway obtain described θ y;

Described accekeration Az is revolved computing anyway obtain described θ z;

Promptly (Ax, Ay Az) have obtained described static foot surface inclining angle (θ x, θ y, θ z) according to described 3-axis acceleration information.

Described angular speed information to collection calculates the angle changing value, specifically comprises:

If r _x(i) be the t=iT angular speed information of x shaft angle rate gyroscope output constantly, r _y(i) be the angular speed information of t=iT y angular rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _XcBe moment x shaft angle degree changing value from t=0 to t=iT, θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To described x shaft angle rate information r _x(i) carry out integration and obtain described angle changing value θ _Xc

To described y shaft angle rate information r _y(i) carry out integration and obtain described angle changing value θ _Yc

To described z shaft angle rate information r _z(i) carry out integration and obtain described angle changing value θ _Zc

Promptly according to described angular speed information (r _x(i), r _y(i), r _z(i)) obtained described angle changing value (θ _Xc, θ _Yc, θ _Zc).

Describedly calculate, obtain real-time foot surface inclining angle information, specifically comprise according to described static foot surface inclining angle information and described angle changing value:

If θ _XtBe the real-time inclination angle between x axle and the surface level, θ _YtBe the real-time foot surface inclining angle between y axle and the surface level, θ _ZtBe the real-time foot surface inclining angle between z axle and the surface level;

θ _X0Be the static foot surface inclining angle at t=0 moment x axle, θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To described θ _X0With described θ _XcSummation obtains the real-time foot surface inclining angle θ of x axle _Xt

To described θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt

To described θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt

Described real-time foot surface inclining angle information is (θ _Xt, θ _Yt, θ _Zt).

Compared with prior art, beneficial effect of the present invention is: humanoid robot foot posture real-time detection provided by the invention and method, measurement foot surface inclining angle information that can be real-time, continuous.

Description of drawings

Fig. 1 is a kind of foot of anthropomorphic robot synoptic diagram that prior art provides;

Fig. 2 is the humanoid robot foot posture real-time detection structural representation that the embodiment of the invention 1 provides;

Fig. 3 is the humanoid robot foot posture real-time detection structural representation that the embodiment of the invention 2 provides;

Fig. 4 is the humanoid robot foot section attitude real-time detection method process flow diagram that the embodiment of the invention 3 provides;

Fig. 5 is the method synoptic diagram by the static foot surface inclining angle of 3-axis acceleration information calculations that the embodiment of the invention provides.

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.

Embodiment 1

Referring to Fig. 2, the embodiment of the invention provides a kind of humanoid robot foot posture real-time detection, described system comprises: DSP (Digital Signal Processing, digital signal processing) controller 11,3-axis acceleration sensor 12, angular rate gyroscope 13.

This dsp controller 11 is used for to 3-axis acceleration sensor 12 write control signals, and the output of 3-axis acceleration sensor 12 is calculated static foot surface inclining angle information;

This dsp controller 11 also is used for to angular rate gyroscope 13 write control signals, and the output of angular rate gyroscope is calculated the angle changing value;

This dsp controller calculates according to the static foot surface inclining angle information and the angle changing value that obtain, obtains real-time foot surface inclining angle information.

Wherein, dsp controller specifically comprises:

First read module 14 is used for reading 3-axis acceleration information from described 3-axis acceleration sensor;

First computing module 15: be used for described 3-axis acceleration information is calculated static foot surface inclining angle information, specifically be used for:

Present embodiment is an example with the x axle, introduces how to calculate static foot surface inclining angle according to acceleration information.Referring to Fig. 5, establish the accekeration of Ax for the output of x axle acceleration sensor, unit is g; θ x is the angle of x axle and surface level; Acceleration of gravity is 1g.

Under quiescent conditions, acceleration transducer only is subjected to the influence of acceleration of gravity, so the accekeration Ax of acceleration transducer output only has relation with the angle theta x of acceleration of gravity 1g and x axle and surface level, and is as follows:

Ax＝sinθx×1g

And acceleration of gravity 1g can think a constant relevant with the geographic position, can be by the acquisition of tabling look-up.Therefore there is unique corresponding relation between Ax and the θ x, as follows:

θx＝arcsin?Ax

Can calculate the angle theta x of x axle and surface level by acceleration A x by following formula.In native system, angle theta x is that corresponding anthropomorphic robot instep is along the static foot surface inclining angle between x direction of principal axis and the surface level.

In like manner, establish the accekeration of Ay for the output of y axle acceleration sensor, Az is the accekeration of z axle acceleration sensor output; θ y is the angle of y axle and surface level, corresponds to the anthropomorphic robot instep along the static foot surface inclining angle between y axle and the surface level, and θ z is the angle of z axle and surface level, corresponds to the anthropomorphic robot instep along the static foot surface inclining angle between z axle and the level;

The y axle is carried out identical with the x axle θ y=arcsin Ay that calculates, the z axle is carried out identical with the x axle θ z=arcsin Az that calculates

By aforementioned calculation, (Az) correspondence becomes static foot surface inclining angle (θ x, θ y, θ z) for Ax, Ay with the acceleration information that obtains.

Wherein, angular rate gyroscope comprises: x shaft angle rate gyroscope 7, and y shaft angle rate gyroscope 8, z shaft angle rate gyroscope 9, dsp controller also comprises:

Second read module 16 is used for reading angular speed information from described angular rate gyroscope;

Second computing module 17 is used for described angular speed information is calculated the angle changing value, specifically is used for: establish r _x(i) be the angular speed information of t=iT angular rate gyroscope output constantly; T is the systematic sampling cycle, is 1ms in native system; θ _RcFor the angle changing value in the moment from t=0 to t=iT, to r _x(i) integration can obtain angle changing value θ _XcPromptly

${\mathrm{\θ}}_{\mathrm{xc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_x\left(i\right)*T$

In like manner, establish r _y(i) be the t=iT angular speed information of y shaft angle rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To y shaft angle rate information r _y(i) use the method identical to carry out integration and obtain described angle changing value θ with the x axle _Yc, promptly

${\mathrm{\θ}}_{\mathrm{yc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_y\left(i\right)*T;$

To z shaft angle rate information r _z(i) use the method identical to carry out integration and obtain described angle changing value θ with the x axle _Zc, promptly

${\mathrm{\θ}}_{\mathrm{zc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_z\left(i\right)*T;$

Promptly according to angular speed information (r _x(i), r _y(i), r _z(i)) obtained angle changing value (θ _Xc, θ _Yc, θ _Zc).

Wherein, dsp controller also comprises summation module 18, is used for calculating according to static foot surface inclining angle information and angle changing value, obtains real-time foot surface inclining angle information, specifically is used for:

If 0 _XtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between x axle and the surface level, θ _YtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between y axle and the surface level, θ _ZtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between z axle and the surface level;

θ _X0Be the static foot surface inclining angle at t=0 moment x axle, θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To described θ _X0With described θ _XcSummation obtains the real-time foot surface inclining angle θ of x axle _Xt, i.e. θ _Xt=θ _X0+ θ _Xc

To described θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt, i.e. θ _Yt=θ _Y0+ θ _Yc

To described θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt, i.e. θ _Zt=θ _Z0+ θ _Zc

Promptly obtain real-time foot surface inclining angle information and be (θ _Xt, θ _Yt, θ _Zt).

Embodiment 2

Referring to Fig. 3, the embodiment of the invention provides a kind of humanoid robot foot posture real-time detection, described system specifically comprises: dsp controller 1,3-axis acceleration sensor 2, angular rate gyroscope, SPI (Serial Peripheral Interface, serial synchronous communication agreement) bus 3, CAN (Controller AreaNetwork, controller local area network) bus 4.

Wherein, dsp controller comprises CAN interface 5 and SPI interface 6;

3-axis acceleration sensor 2 links to each other with the SPI interface 6 of dsp controller 1 by spi bus 3, is used to measure x, the y of mutually orthogonal, the acceleration of z axle; X shaft angle rate gyroscope 7, y shaft angle rate gyroscope 8 and z shaft angle rate gyroscope 9 link to each other with the SPI interface 6 of dsp controller 1 by spi bus 3 respectively, are used for the angular speed information of measurement of x, y, z axle.

The model of the 3-axis acceleration sensor that adopts in the present embodiment is MMA7455L, and MMA7455L is a kind of 3 axis MEMS (Micro-Electro-Mechanical Systems, MEMS (micro electro mechanical system)) acceleration transducer.

The model of x shaft angle rate gyroscope 7, y shaft angle rate gyroscope 8 and z shaft angle rate gyroscope 9 all is ADIS16255, and ADIS16255 is a kind of single shaft MEMS angular rate sensor.

Dsp controller 1 can be by SPI interface 6 to 3-axis acceleration sensor 2MMA7455L write control signal, comprise measurement range selection, frequency acquisition, calibrating parameters etc., simultaneously, dsp controller 1 can be read 3-axis acceleration information with digital form by SPI interface 6, and 3-axis acceleration information is calculated static foot surface inclining angle information.Concrete computing method are as follows:

Present embodiment is an example with the x axle, introduces how to calculate static foot surface inclining angle according to acceleration information.Referring to Fig. 5, establish the accekeration of Ax for the output of x axle acceleration sensor, unit is g; θ x is the angle of x axle and surface level; Acceleration of gravity is 1g.

Under quiescent conditions, acceleration transducer only is subjected to the influence of acceleration of gravity, so the accekeration Ax of acceleration transducer output only has relation with the angle theta x of acceleration of gravity 1g and x axle and surface level, and is as follows:

Ax＝sinθx×1g

And acceleration of gravity 1g can think a constant relevant with the geographic position, can be by the acquisition of tabling look-up.Therefore there is unique corresponding relation between Ax and the θ x, as follows:

θx＝arcsin?Ax

Can calculate the angle theta x of x axle and surface level by acceleration A x by following formula.In native system, angle theta x is that corresponding anthropomorphic robot instep is along the static foot surface inclining angle between x direction of principal axis and the surface level.

In like manner, establish the accekeration of Ay for the output of y axle acceleration sensor, Az is the accekeration of z axle acceleration sensor output; θ y is the angle of y axle and surface level, corresponds to the anthropomorphic robot instep along the static foot surface inclining angle between y axle and the surface level, and θ z is the angle of z axle and surface level, corresponds to the anthropomorphic robot instep along the static foot surface inclining angle between z axle and the surface level;

The y axle is carried out identical with the x axle θ y=arcsin Ay that calculates, the z axle is carried out identical with the x axle θ z=arcsin Az that calculates

By aforementioned calculation, (Az) correspondence becomes static foot surface inclining angle (θ x, θ y, θ z) for Ax, Ay with the acceleration information that obtains.

Dsp controller 1 can be by SPI interface 6 to x shaft angle rate gyroscope 7, y shaft angle rate gyroscope 8 and z shaft angle rate gyroscope 9 difference write control signals, comprise measurement range selection, frequency acquisition, calibrating parameters etc., simultaneously, dsp controller 1 can be read x shaft angle rate information, y shaft angle rate information, z shaft angle rate information respectively with digital form by SPI interface 6; And x shaft angle rate information, y shaft angle rate information, the z shaft angle rate information that obtains calculated the angle changing value respectively.Concrete computing method are as follows:

Present embodiment is an example with the x axle still, introduces how the tri-axis angular rate information of gathering to be calculated the angle changing value.

If r _x(i) be the angular speed information of t=iT angular rate gyroscope output constantly; T is the systematic sampling cycle, is 1ms in native system; θ _XcFor the angle changing value in the moment from t=0 to t=iT, to r _x(i) integration can obtain angle changing value θ _Xc, promptly

${\mathrm{\θ}}_{\mathrm{xc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_x\left(i\right)*T$

In like manner, establish r _y(i) be the t=iT angular speed information of y shaft angle rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To y shaft angle rate information r _y(i) use the method identical to carry out integration and obtain described angle changing value θ with the x axle _Yc, promptly

${\mathrm{\θ}}_{\mathrm{yc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_y\left(i\right)*T$

To z shaft angle rate information r _z(i) use the method identical to carry out integration and obtain described angle changing value θ with the x axle _Zc, promptly

${\mathrm{\θ}}_{\mathrm{zc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_z\left(i\right)*T;$

Promptly according to angular speed information (r _x(i), r _y(i), r _z(i)) obtained angle changing value (θ _Xc, θ _Yc, θ _Zc).

Then, dsp controller calculates according to static foot surface inclining angle information and angle changing value, obtains real-time, the continuous foot surface inclining angle information of robot.Concrete computing method are as follows:

Present embodiment is an example with the x axle still, introduces how to calculate real-time foot surface inclining angle information according to static foot surface inclining angle information and angle changing value;

If θ _XtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between x axle and the surface level; θ _X0For calculate the static foot surface inclining angle of x axle constantly at t=0; To θ _X0With θ _XcSummation can obtain the real-time foot surface inclining angle θ of x axle _Xt, i.e. θ _Xt=θ _X0+ θ _Xc

In like manner, establish θ _YtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between y axle and the surface level, θ _ZtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between z axle and the surface level; θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt, i.e. θ _Yt=θ _Y0+ θ _Yc

To θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt, i.e. θ _Zt=θ _Z0+ θ _Zc

Promptly obtain real-time foot surface inclining angle information (θ _Xt, θ _Yt, θ _Zt).

The CAN interface 5 of dsp controller 1 links to each other with anthropomorphic robot motion control computing machine 11 by CAN bus 4.

Anthropomorphic robot motion control computing machine 11 is cores of anthropomorphic robot motion control, general powerful industrial computer and the special-purpose real time operating system of adopting, foot gesture real-time detection system 10 links to each other with anthropomorphic robot motion control computing machine 11 by CAN bus 4, send the real-time foot gesture information of anthropomorphic robot to anthropomorphic robot motion control computing machine 11, comprise real-time foot surface inclining angle information, 3-axis acceleration information and angle changing value, 3-axis acceleration information also can be real-time foot acceleration information, and angular speed information also can be real-time foot rotation angle rate information.

The detection frequency of this humanoid robot foot posture real-time detection and signal output frequency are 1kHz.

The beneficial effect of the embodiment of the invention is: by the 3-axis acceleration information that collects is in real time calculated, obtain static foot surface inclining angle information, by the angular speed information that collects is in real time calculated the angle changing value, obtain real-time, the continuous foot surface inclining angle information of real-time robot by static foot surface inclining angle information and the angle changing value that obtains again.

Embodiment 3

Referring to Fig. 4, the embodiment of the invention provides a kind of real-time detection method of humanoid robot foot section attitude information, specifically comprises:

Step 101: each sensor of initialization.

Step 102: gather 3-axis acceleration information.

Step 103: the 3-axis acceleration information of gathering is calculated the static instep pin information of inclining.

Present embodiment is an example with the x axle, introduces the method for how calculating static foot surface inclining angle according to acceleration information.Referring to Fig. 5, establish the accekeration of Ax for the output of x axle acceleration sensor, unit is g; θ x is the angle of x axle and surface level; Acceleration of gravity is 1g.

Under quiescent conditions, acceleration transducer only is subjected to the influence of acceleration of gravity, so the accekeration Ax of acceleration transducer output only has relation with the angle theta x of acceleration of gravity 1g and x axle and surface level, and is as follows:

Ax＝sinθx×1g

And acceleration of gravity 1g can think a constant relevant with the geographic position, can be by the acquisition of tabling look-up.Therefore there is unique corresponding relation between Ax and the θ x, as follows:

θx＝arcsin?Ax

Can calculate the angle theta x of x axle and surface level by acceleration A x by following formula.In the method, angle theta x is that corresponding anthropomorphic robot instep is along the static foot surface inclining angle between x direction of principal axis and the surface level.

In like manner, establish the accekeration of Ay for the output of y axle acceleration sensor, Az is the accekeration of z axle acceleration sensor output; θ y is the angle of y axle and surface level, corresponds to the anthropomorphic robot instep along the static foot surface inclining angle between y axle and the surface level, and θ z is the angle of z axle and surface level, corresponds to the anthropomorphic robot instep along the static foot surface inclining angle between z axle and the surface level;

The y axle is carried out identical with the x axle θ y=arcsin Ay that calculates,

The z axle is carried out identical with the x axle θ z=arcsin Az that calculates,

By aforementioned calculation, (Az) correspondence becomes static foot surface inclining angle (θ x, θ y, θ z) for Ax, Ay with the acceleration information that obtains.

Step 104: gather tri-axis angular rate information.

Step 105: the tri-axis angular rate information of gathering is calculated the angle changing value.

Present embodiment is an example with the x axle still, introduces how the tri-axis angular rate information of gathering to be calculated the angle changing value.

If r _x(i) be the angular speed information of t=iT angular rate gyroscope output constantly; T is the systematic sampling cycle, is 1ms in the method; θ _XcFor the angle changing value in the moment from t=0 to t=iT, to r _x(i) integration can obtain angle changing value θ _Xc, promptly

${\mathrm{\θ}}_{\mathrm{xc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_x\left(i\right)*T$

In like manner, establish r _y(i) be the t=iT angular speed information of y shaft angle rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To y shaft angle rate information r _y(i) use the method identical to carry out integration and obtain described angle changing value θ with the x axle _Yc, promptly

${\mathrm{\θ}}_{\mathrm{yc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_y\left(i\right)*T;$

To z shaft angle rate information r _z(i) use the method identical to carry out integration and obtain described angle changing value θ with the x axle _Zc, promptly

${\mathrm{\θ}}_{\mathrm{zc}}=\underset{i=0}{\overset{N}{\mathrm{\Σ}}}{r}_z\left(i\right)*T;$

Promptly according to angular speed information (r _x(i), r _y(i), r _z(i)) obtained angle changing value (θ _Xc, θ _Yc, θ _Zc).

Step 106: the angle changing value that static foot surface inclining angle information that obtains according to step 103 and step 105 obtain calculates, and obtains real-time foot surface inclining angle information, returns step 104 simultaneously.

Present embodiment is an example with the x axle still, introduces how to calculate real-time foot surface inclining angle information according to static foot surface inclining angle and angle changing value;

If θ _XtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between x axle and the surface level; θ _X0Static foot surface inclining angle for the x axle that calculates constantly at t=0; To θ _X0With θ _XcSummation can obtain the real-time foot surface inclining angle θ of x axle _Xt, i.e. θ _Xt=θ _X0+ θ _Xc

In like manner, establish θ _YtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between y axle and the surface level, θ _ZtFor the anthropomorphic robot instep along the real-time foot surface inclining angle between z axle and the surface level; θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt, i.e. θ _Yt=θ _Y0+ θ _Yc

To θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt, i.e. θ _Zt=θ _Z0+ θ _Zc

Promptly obtain real-time foot surface inclining angle information (θ _Xt, θ _Yt, θ _Zt).

Detection frequency in this humanoid robot foot section attitude real-time detection method and signal output frequency are 1kHz.

Beneficial effect of the present invention is: calculate by the 3-axis acceleration information that Real-time Collection is obtained, obtain robot The static instep pin information of inclining calculates robot angle changing value by the angular speed information that Real-time Collection is obtained, again Obtain real-time, the continuous foot surface inclining angle information of real-time robot by incline pin information and angle changing value of the static instep that obtains.

Claims (9)

1. a humanoid robot foot posture real-time detection is characterized in that, described system comprises:

Dsp controller, 3-axis acceleration sensor, angular rate gyroscope;

Described dsp controller is used for to described 3-axis acceleration sensor write control signal, and the output of described 3-axis acceleration sensor is calculated static foot surface inclining angle information;

Described dsp controller is used for to described angular rate gyroscope write control signal, and the output of described angular rate gyroscope is calculated the angle changing value;

Described dsp controller calculates according to described static foot surface inclining angle information and angle changing value, obtains real-time foot surface inclining angle information.

2. humanoid robot foot posture real-time detection according to claim 1 is characterized in that, described dsp controller specifically comprises:

First read module is used for reading 3-axis acceleration information from described 3-axis acceleration sensor;

First computing module is used for described 3-axis acceleration information is calculated static foot surface inclining angle information, specifically is used for:

If Ax is the accekeration of x axle acceleration sensor output, Ay is the accekeration of y axle acceleration sensor output, and Az is the accekeration of z axle acceleration sensor output; θ x is x axle and horizontal plane angle, and θ y is y axle and horizontal plane angle, and θ z is z axle and horizontal plane angle;

Described accekeration Ax is revolved computing anyway obtain described θ x;

Described accekeration Ay is revolved computing anyway obtain described θ y;

Described accekeration Az is revolved computing anyway obtain described θ z;

Promptly (Ax, Ay Az) have obtained described static foot surface inclining angle (θ x, θ y, θ z) according to described 3-axis acceleration information.

3. humanoid robot foot posture real-time detection according to claim 1 and 2 is characterized in that, described angular rate gyroscope comprises x shaft angle rate gyroscope, y shaft angle rate gyroscope and z shaft angle rate gyroscope; Described dsp controller specifically comprises:

Second read module is used for reading angular speed information from described angular rate gyroscope;

Second computing module is used for described angular speed information is calculated the angle changing value, specifically is used for:

If r _x(i) be the t=iT angular speed information of x shaft angle rate gyroscope output constantly, r _y(i) be the t=iT angular speed information of y shaft angle rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _XcBe moment x shaft angle degree changing value from t=0 to t=iT, θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To described x shaft angle rate information r _x(i) carry out integration and obtain described angle changing value θ _Xc

To described y shaft angle rate information r _y(i) carry out integration and obtain described angle changing value θ _Yc

To described z shaft angle rate information r _z(i) carry out integration and obtain described angle changing value θ _Zc

Promptly according to described angular speed information (r _x(i), r _y(i), r _z(i)) obtained described angle changing value (θ _Xc, θ _Yc, θ _Zc).

4. humanoid robot foot posture real-time detection according to claim 3, it is characterized in that described dsp controller also comprises summation module, be used for calculating according to described static foot surface inclining angle information and angle changing value, obtain real-time foot surface inclining angle information, specifically be used for:

If θ _XtBe the real-time inclination angle between x axle and the surface level, θ _YtBe the real-time foot surface inclining angle between y axle and the surface level, θ _ZtBe the real-time foot surface inclining angle between z axle and the surface level;

θ _X0Be the static foot surface inclining angle at t=0 moment x axle, θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To described θ _X0With described θ _XcSummation obtains the real-time foot surface inclining angle θ of x axle _Xt

To described θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt

To described θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt

Described real-time foot surface inclining angle information is (θ _Xt, θ _Yt, θ _Zt).

5. humanoid robot foot posture real-time detection according to claim 3 is characterized in that, described angular speed information is x shaft angle rate information, y shaft angle rate information and z shaft angle rate information.

6. humanoid robot foot section attitude real-time detection method is characterized in that described method comprises:

The 3-axis acceleration information of gathering is calculated static foot surface inclining angle information;

The angular speed information of gathering is calculated the angle changing value;

Calculate according to described static foot surface inclining angle information and described angle changing value, obtain real-time foot surface inclining angle information.

7. humanoid robot foot section attitude real-time detection method according to claim 6 is characterized in that, described 3-axis acceleration information to collection calculates static foot surface inclining angle information, specifically comprises:

If Ax is the accekeration of x axle acceleration sensor output, Ay is the accekeration of y axle acceleration sensor output, and unit is g, and Az is the accekeration of z axle acceleration sensor output; θ x is x axle and horizontal plane angle, and θ y is y axle and horizontal plane angle, and θ z is z axle and horizontal plane angle;

Described accekeration Ax is revolved computing anyway obtain described θ x;

Described accekeration Ay is revolved computing anyway obtain described θ y;

Described accekeration Az is revolved computing anyway obtain described θ z;

Promptly (Ax, Ay Az) have obtained described static foot surface inclining angle (θ x, θ y, θ z) according to described 3-axis acceleration information.

8. according to claim 6 or 7 described humanoid robot foot section attitude real-time detection methods, it is characterized in that described angular speed information to collection calculates the angle changing value, specifically comprises:

If r _x(i) be the t=iT angular speed information of x shaft angle rate gyroscope output constantly, r _y(i) be the angular speed information of t=iT y angular rate gyroscope output constantly, r _z(i) be the t=iT angular speed information of z shaft angle rate gyroscope output constantly; θ _XcBe moment x shaft angle degree changing value from t=0 to t=iT, θ _YcBe moment y shaft angle degree changing value from t=0 to t=iT, θ _ZcBe moment z shaft angle degree changing value from t=0 to t=iT;

To described x shaft angle rate information r _x(i) carry out integration and obtain described angle changing value θ _Xc

To described y shaft angle rate information r _y(i) carry out integration and obtain described angle changing value θ _Yc

To described z shaft angle rate information r _z(i) carry out integration and obtain described angle changing value θ _Zc

Promptly according to described angular speed information (r _x(i), r _y(i), r _z(i)) obtained described angle changing value (θ _Xc, θ _Yc, θ _Zc).

9. humanoid robot foot section attitude real-time detection method according to claim 8 is characterized in that, describedly calculates according to described static foot surface inclining angle information and described angle changing value, obtains real-time foot surface inclining angle information, specifically comprises:

If θ _XtBe the real-time inclination angle between x axle and the surface level, θ _YtBe the real-time foot surface inclining angle between y axle and the surface level, θ _ZtBe the real-time foot surface inclining angle between z axle and the surface level;

θ _X0Be the static foot surface inclining angle at t=0 moment x axle, θ _Y0Be the static foot surface inclining angle at t=0 moment y axle, θ _Z0Be static foot surface inclining angle at t=0 moment z axle;

To described θ _X0With described θ _XcSummation obtains the real-time foot surface inclining angle θ of x axle _Xt

To described θ _Y0With described θ _YcSummation obtains the real-time foot surface inclining angle θ of y axle _Yt

To described θ _Z0With described θ _ZcSummation obtains the real-time foot surface inclining angle θ of z axle _Zt

Described real-time foot surface inclining angle information is (θ _Xt, θ _Yt, θ _Zt).

Images