CN105005303A - Vertical and balanced robot - Google Patents

Abstract

The present invention discloses a vertical and balanced robot, comprising a body, wherein an accelerometer, a gyroscope, and a control module integrating a stepping motor driving unit are arranged inside the body, two sides of the lower end of the body are provided with two rolling wheels, two sets of stepping motors are arranged inside the body, two sides of the body are provided with movable arms, the control module performs filtering and calculating for detection quantities of the accelerometer and the gyroscope to obtain an angle value and an angular velocity value, performs PD control to generate two output quantities used for controlling rotation of the stepping motors by utilizing the angle value and the angular velocity value, and controls operation of the stepping motors through the stepping motor driving unit. According to the vertical and balanced robot of the present invention, the robot can be accurately controlled to keep vertical and balanced through control for rotation of the two motors; the integral body of the robot is in a human shape, and has a bionic shape effect; the control module integrates the stepping motor driving unit, and a traditional large motor driver is not required, thus the volume of the robot can be reduced, and power consumption is reduced; and the robot can be further configured to have an entertainment function.

Description

Vertical balanced robot

Technical field

The present invention relates to electronic technology field, be specifically related to a kind of vertical balanced robot for teaching.

Background technology

There is no a kind of specially for the special upright robot that teaching uses in prior art.Therefore, be necessary a kind of corresponding robot product of exploitation, use for student's Fast Learning Robotics.

Summary of the invention

In order to realize above object, the invention provides a kind of vertical balanced robot, it can meet the request for utilization of teaching and amusement simultaneously.

The present invention is achieved by the following technical solutions:

Vertical balanced robot, comprise the fuselage of overall one-tenth erectility, the inside of fuselage is provided with the accelerometer for detection angle angle value, for the gyroscope of detection angle velocity amplitude and the control module of integrated driving stepper motor unit, the both sides of fuselage lower end are provided with for two rollers with earth surface, fuselage interior is provided with the two groups of stepper motors individually driving two rollers to rotate, the both sides of described fuselage are provided with the lever arm of controlled module control activity, described control module based on accelerometer and gyrostatic detection limit after filtering, angle value and magnitude of angular velocity is obtained after calculating, utilize angle value and magnitude of angular velocity to carry out PD and control generation two for controlling the output quantity of electric machine rotation, worked by driving stepper motor unit controls stepper motor.

As preferred embodiment, described for the wireless data transfer module with external communication, wireless data transfer module and control module are electrically connected.

As preferred embodiment, the outside of described fuselage is provided with temperature sensor, humidity sensor, pressure transducer and sound transducer, and temperature sensor, humidity sensor, pressure transducer are connected with control module with the output terminal of sound transducer.

As preferred embodiment, the outside of described fuselage is provided with the impression sensor for responding to other robot, and the output terminal experiencing sensor is connected with control module.Further, described lever arm is provided with acousto-optic rifle, when fuselage front senses other robot, acousto-optic rifle is lighted.

The invention has the beneficial effects as follows: the present invention can be accurately kept upright balance by control by the rotation of control two motors; Its entirety is humanoid moulding, has bionic shape effect; Control module is integrated driving stepper motor unit, does not need to arrange traditional heavy motor driver, reduces the volume of robot itself, reduces power consumption; This robot can configure further and make to have amusement function.

Accompanying drawing explanation

Fig. 1 is overall appearance structural drawing of the present invention;

Fig. 2 is the block diagram of system of the present invention.

Symbol description: 1-fuselage, 2-roller, 3-stepper motor, 4-lever arm, 5-accelerometer, 6-gyroscope, 7-control module, 8-wireless data transfer module, 9-environmental sensor, 10-experiences sensor.

Embodiment

With reference to the accompanying drawings and the invention will be further described in conjunction with the embodiments.

With reference to Fig. 1, vertical balanced robot of the present invention entirety is humanoid moulding, comprises fuselage 1, two rollers 2 and two groups of stepper motors 3 being respectively used to control roller 2 and rotating in appearance.Fuselage 1 entirety becomes erectility.During work, stepper motor 3 controls the different rotating speed of two rollers 2 and realizes robot vertical balanced and move and rotate.In addition, the both sides of fuselage 1 are also provided with mobilizable lever arm 4, and lever arm 4 can realize various action under control.

As shown in Figure 2, accelerometer 5, gyroscope 6, control module 7 etc. are provided with in robot.Accelerometer 5 is for detecting the angle value of fuselage, and gyroscope 6 is for detecting the magnitude of angular velocity of fuselage.Be integrated with driving stepper motor unit in control module 7, therefore control module 7 can work by Direct driver stepper motor 3, does not need to arrange extra driver.During work, control module 7 based on accelerometer 5 and gyroscope 6 detection limit after filtering, calculate after obtain angle value and magnitude of angular velocity, utilize angle value and magnitude of angular velocity to carry out PD and control generation two for controlling the output quantity of electric machine rotation, worked by driving stepper motor unit controls stepper motor.

The angle used in actual robot and angular speed calculation method as follows:

float Angle = 0, Angle_dot = 0;

float Q_angle=0.001, Q_gyro=0.003, R_angle=0.67, dt=0.0025;

float P[2][2] ={{ 1, 0 },{ 0, 1 }};

char C_0 = 1;

float E = 0;

float q_bias = 0;

float Angle_err = 0;

float PCt_0 = 0, PCt_1 = 0;

float K_0 = 0, K_1 = 0;

float t_0 = 0, t_1 = 0;

float Pdot[4] ={0,0,0,0};

void filter(float angle_m,float gyro_m)

{

Angle += (gyro_m-q_bias) * dt;

Pdot[0]=Q_angle - P[0][1] - P[1][0];

Pdot[1]=- P[1][1];

Pdot[2]=- P[1][1];

Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

Angle_err = angle_m - Angle;

PCt_0 = C_0 * P[0][0];

PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

Angle += K_0 * Angle_err;

q_bias+= K_1 * Angle_err;

Angle_dot = gyro_m-q_bias;

}

Wherein, function parameter is that sensor records metadata (through identity transformation), Angle and Angle_dot is final filtering Output rusults, is respectively angle and angular velocity.Dt is the filter sample time, and all the other are intermediate variable.

After obtaining concrete angle and angular velocity information, typical PD is adopted to control to produce output quantity.

duty_PD=Kp_Angle *Angle_Err+Kd_Angle * Angle_V;

Wherein Angle_Err is angular deviation, and Angle_V is angular velocity.

Carefully adjust Kp_Angle and Kd_Angle again, robot just can realize uprightly.

Robot of the present invention can by built-in more module to realize better teaching and amusement function.

Such as, can be provided with for the wireless data transfer module 8 with external communication in fuselage, wireless data transfer module 8 and control module 7 are electrically connected.Utilize this wireless data transfer module 8 can carry out communication with outside easily, outside can to robotic transfer's control command, control machine human action.

Again such as, the outside of fuselage can be provided with the environmental sensors 9 such as temperature sensor, humidity sensor, pressure transducer and sound transducer, and temperature sensor, humidity sensor, pressure transducer are connected with control module 7 with the output terminal of sound transducer.Utilize above-mentioned various sensor can detect environment residing for robot, the data of detection are delivered in control module 7, thus control realizes various action.In addition, these data also outwards can be transmitted by wireless data transfer module 8, play the effect utilizing robot probe's environment.

Again such as, the outside of fuselage can be provided with the impression sensor 10 for responding to other robot, and the output terminal experiencing sensor 10 is connected with control module 7.Corresponding corresponding actions can be made when robot senses other robot.As lever arm being provided with acousto-optic rifle, when fuselage front senses other robot, acousto-optic rifle is lighted, to realize the function simulating battle.

The foregoing is only the preferred embodiments of the present invention, be not limited to the present invention, for a person skilled in the art, the present invention can have various modifications and variations.Within the spirit and principles in the present invention all, any amendment done, equivalent replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (5)

1. vertical balanced robot, it is characterized in that: the fuselage comprising overall one-tenth erectility, the inside of fuselage is provided with the accelerometer for detection angle angle value, for the gyroscope of detection angle velocity amplitude and the control module of integrated driving stepper motor unit, the both sides of fuselage lower end are provided with for two rollers with earth surface, fuselage interior is provided with the two groups of stepper motors individually driving two rollers to rotate, the both sides of described fuselage are provided with the lever arm of controlled module control activity, described control module based on accelerometer and gyrostatic detection limit after filtering, angle value and magnitude of angular velocity is obtained after calculating, utilize angle value and magnitude of angular velocity to carry out PD and control generation two for controlling the output quantity of electric machine rotation, worked by driving stepper motor unit controls stepper motor.

2. vertical balanced robot according to claim 1, is characterized in that: described for the wireless data transfer module with external communication, and wireless data transfer module and control module are electrically connected.

3. vertical balanced robot according to claim 1, it is characterized in that: the outside of described fuselage is provided with temperature sensor, humidity sensor, pressure transducer and sound transducer, and temperature sensor, humidity sensor, pressure transducer are connected with control module with the output terminal of sound transducer.

4. vertical balanced robot according to claim 1, is characterized in that: the outside of described fuselage is provided with the impression sensor for responding to other robot, and the output terminal experiencing sensor is connected with control module.

5. vertical balanced robot according to claim 4, is characterized in that: described lever arm is provided with acousto-optic rifle, and when fuselage front senses other robot, acousto-optic rifle is lighted.