CN103170962A - Desktop type double-wheel self-balancing robot - Google Patents

Abstract

The invention discloses a desktop type double-wheel self-balancing robot, and belongs to the field of robot controlling. The robot comprises four modules which are a main control module, a power module, a sensing module and an executing module. The robot senses the current angle of the robot through an attitude sensor, and a control chip is enabled to control a motor to move through a series of algorithms to maintain the balance of the robot. A Hall sensor is used to carry out displacement measurement, and displacement differential is carried out with time to obtain the speed of the robot. An ultrasonic sensor is used to detect obstacle information, and an infrared sensor is used to measure the movement locus of the robot, and a series of functions of fixed point balancing, obstacle avoidance, tracking and the like of the robot are achieved. The desktop type double-wheel self-balancing robot has the advantages of being small in size, low in cost, abundant in functions, building-block-designed, strong in expansion capability and the like, and the application range of the desktop type double-wheel self-balancing robot is wider.

Description

Desktop type Double-wheel self-balancing robot

Technical field

The present invention is a kind of desktop type Double-wheel self-balancing robot, belongs to the robot control field.

Background technology

Along with deepening continuously of mobile robot research, the Double-wheel self-balancing robot has become the study hotspot in robot research field.The characteristics of two-wheel robot maximum are, it can realize self-balancing, and are flexibly mobile, more intense to the environmental change adaptive capacity, compare with many wheel robots, and it can the original place rotation, turn to more flexibly, take action sensitiveer.

Segway(Sai Gewei) be the representative of two-wheel robot, it is to measure the current angle state of vehicle body with the gyroscope of precision, and after processing the gained attitude data by cpu chip at a high speed, drive motors is made corresponding sports to reach counterbalance effect.Segway is widely used in the supermarket, the public arenas such as airport after coming out.

With Segway(Sai Gewei) etc. some large-scale two-wheels robots compare, desktop type two-wheel robot has practicality widely.At first, the modular design of desktop type robot makes it have stronger autgmentability; Secondly, less volume makes it can be applied to more field, such as to the exploration in the cave of some less etc.; Again, its control algolithm is also simple than large-scale robot; The miniaturization of last robot makes its cost material benefit more economically, and cost is cheaper.

Summary of the invention

Characteristics of the present invention are that its structure adopts modularized design, is comprised of four modules altogether: main control module, and Executive Module, sensing module and power module, its feature richness, extensibility is strong.

structure of the present invention comprises: main body rack 1, be fixed on two electric machine supports 11 of main body rack 1 below, be fixed in two motors 12 on two electric machine supports 11, be individually fixed in two wheels 9 on two motors 12, be fixed in the small magnet 15 of motor tail end, be fixed in upper layer bracket 2 on main body rack 1 by copper post 4, be fixed in the control chip 3 between main body rack 1 and upper layer bracket 2, be fixed in battery 5 and attitude transducer 10 on main body rack 1, be placed in the electric pressure converter 7 on upper layer bracket 2, ultrasonic sensor 8 and bluetooth module 6, and be fixed in Hall element 14 between the motor 12 of main body rack two of 1 belows, two infrared sensors 13 that also have two motor 12 the place aheads.Battery 5 connects electric pressure converter 7, be main control chip 3 and motor-drive circuit power supply, the output of ultrasonic sensor 8 and infrared sensor 13 and Hall element 14 is connected with the input of main control chip 3 respectively, and the output of main control chip 3 is connected with motor 12.

The present invention can obtain following function:

1) free balance.The attitude angle that attitude transducer 10 detection machine people are current transfers data to control chip 3, and control chip 3 draws the PWM ripple and controls motor 12 motions, makes robot keep balance.

2) fixed point balance.Keep under the robot equipoise, promote robot to last segment distance, Hall element 14 records robot motion's concrete displacement, and control chip 3 is controlled motor 12 and moved backward identical position, makes robot get back to origin position.

3) keep away barrier.Robot moves under poised state, and ultrasonic sensor 8 is detection machine people the place ahead road conditions ceaselessly, when ultrasonic sensor 8 detects the place ahead barrier is arranged, control chip 3 is controlled respectively the speed of two motors 12, realize that robot turns, avoid the place ahead barrier, realize keeping away barrier.

4) tracking.Robot is in the process of motion, two infrared sensors 13 that are placed in the front lower place utilize infrared ray to have the characteristics of different reflected intensitys at the body surface of different colours, track is identified, and control chip 3 is controlled motor 12 motions, makes robot along set track walking.

5) Remote.Bluetooth module 6 is installed, it and mobile phone and notebook Bluetooth pairing in robot.

The present invention is simple in structure, has several functions, and has expansibility preferably.

Description of drawings

Fig. 1 is the robot overall construction drawing;

Fig. 2 is TV structure figure under robot;

Fig. 3 is the system construction drawing of robot;

Fig. 4 ultrasonic sensor fundamental diagram;

Fig. 5 infrared sensor fundamental diagram I;

Fig. 6 infrared sensor fundamental diagram II;

Fig. 6 robot tracking schematic diagram.

Fig. 7 robot tracking schematic diagram.

In figure: 1-lower floor support, 2-upper layer bracket, 3-main control chip, 4-copper post, 5-battery, the 6-bluetooth module, 7-electric pressure converter, 8-ultrasonic sensor, 9-wheel, 10-attitude transducer, the 11-electric machine support, 12-motor, 13-infrared sensor, 14-Hall element, 15-small magnet.

The specific embodiment

Below in conjunction with accompanying drawing, the present invention is further illustrated.

At first frame for movement of the present invention is elaborated.As shown in Figure 1 and Figure 2, frame for movement of the present invention adopts layered structure, and cost is low, easily realizes, easily expansion.Lower floor support 1 and the upper layer bracket 2 of robot all adopt the resin glass plate, more traditional metab, and the resin glass plate is lighter, and easily punching, facilitates robot Function Extension in the future.Electric machine support 11 is fixed to the lower end of robot lower carriage 1 by each 4 screws of left and right, then motor 12 is fixed on electric machine support 11 by each two fixed screws up and down, the wheel 9 of robot is fixed on motor 12, and motor 12 tail ends have been fixed the small magnet 15 that detecting position is moved to.What wheel 9 adopted is the plastics of hard, and plastic outer ring is applied mechanically the tire that rubber is made, and compared with the wheel of common standard, this wheel is lighter, can greatly alleviate the burden of motor, thereby has lowered the power of motor, and then has reduced the cost of motor.Upper layer bracket 2 is fixed on lower floor support 1 by four long copper posts 4, and the above is fixed with electric pressure converter 6 and ultrasonic sensor 7, and the arrangement of convenient robot Function Extension in the future and other various kinds of sensors.

Below in conjunction with Fig. 3, hardware system structure of the present invention is described.The present invention is comprised of four modules altogether: main control module, Executive Module, sensing module and power module.Wherein main control module adopts DFRduinoRoMeo Master control chip 3, and Executive Module is DC speed-reducing 12, and power module comprises battery 5 and electric pressure converter 7, sensing module comprises attitude transducer 10, ultrasonic sensor 8, Hall element 14, infrared sensor 13, bluetooth module 6.

The RoMeo Master control chip 3 of main control module be one based on the SimpleI/O platform of Arduino open source code, have and use similar java, the development environment of C language.Integrated 2 road motor drive modules (needing power supply separately), wireless data sending and DF-Bluetooth blue tooth interface, a plurality of numerals and Simulation with I O expansion mouth satisfy the requirement that robot realizes each function.

The DC speed-reducing 12 of Executive Module is the high pulling torque reducing motor, and rated voltage is 6V, and rated speed is 150 rev/mins, and torsion is 6KG/CM, weight 85G, and motor rear end output axial length is 5 millimeters, and small magnet 15 can be installed.Motor not only satisfies the needed power performance requirement of robot, and facilitates the installation of speed measuring device.

In power module, battery 5 adopts the lithium battery of 7.4V, not only satisfy the power reguirements of components and parts, and light little convenience, voltage transitions by electric pressure converter 7, can give respectively the power supply of motor drive module and main control chip, and master chip has different voltage outputs, satisfies the power demands of each sensor.

That in sensing module, attitude transducer 10 adopts is three axis accelerometer and three-axis gyroscope Fusion Module MPU6050, and volume is little, is easy to install, and cost performance is very high, and the I2C communication protocol of standard facilitates for the connection of circuit provides greatly.Displacement transducer 14 is used Hall element, comprise the magnetic detection element, the little operating distance of volume is long, signal stabilization, reliability is high, and frequency range is wide, can detect simultaneously the rotation number of turns of two motors 10, thereby change into the displacement of robot, contraposition shift-in line time differential can obtain the speed of robot.Keep away barrier sensor 8 and adopted the PING of Parallax company))) ultrasonic sensor, its minimum detection distance is 2CM, maximum detection distance is 3M.What tracking sensor 13 adopted is the TCRT5000 infrared sensor, and it is highly sensitive and adjustable, and it is convenient that the four-wire system interface connects circuit, can effectively be used for the check of form and aspect material, so can well realize the tracking function.Wireless remote operational module 6 adopts the JY-Bluetooth bluetooth module, and its Style Design is simple, and volume size is compact, the LINK indicator lamp is clear bright, be used for the display module duty, carry efficient plate and carry antenna, antenna exposes and makes the better transmitting range of signal quality farther.

The detailed process that each function of comprehensive marginal data robot of the present invention realizes:

1) free balance.Self-balancing is that the two-wheel robot is substantially the most also most important feature.The present invention adopts the current attitude angle of MPU6050 detection machine people, utilizes the X-axis of three axis accelerometer and Z axis to carry out antitrigonometric function and calculates the current angle that departs from the equilbrium position of robot:

angle_acc=arctan(ax/az)*180/π

Wherein angle_acc represents the robot deviation angle of accelerometer measures, and ax, az are respectively the instant measured value of accelerometer X-axis Z axis.

Utilize the current angular speed of Y-axis robot measurement of three-axis gyroscope:

Omega=(gy-Gy_offset)/Gy_gain

Wherein Omega represents the current angular speed of robot, and gy is the instant measured value of gyroscope Y-axis, and Gy_offset is the full scale side-play amount of gyroscope Y-axis, and Gy_gain is the scale factor of gyroscope Y-axis.

Then by the single order complementary filter, with angle and angular speed fused filtering:

Angle=a*(Anle+Omega*dt)+(1-a)*angle_acc

Wherein Angle represents the angle value that obtains after filtering, and a is determined by timeconstantτ and sample frequency dt:

$a=\frac{\mathrm{\τ}}{\mathrm{\τ}+\mathrm{dt}}$

Angle A ngle after merging is transferred to control chip 3, and control chip 3 is exported corresponding PWM ripple and is controlled motor rotation through after calculating, and keeps the robot balance.For example the robot front is positive direction) when attitude angle greater than 0 the time, the robot center of gravity leans forward, the accelerated motion forward of chip controls motor makes robot reach balance in new position.

The balancing control algorithm of this novel employing is the NONLINEAR PD algorithm, and its expression formula can be expressed as:

$\mathrm{PD}\left(\mathrm{\θ}\right)=K_P\left(\mathrm{\θ}\right)\mathrm{\θ}+K_D\stackrel{\·}{\mathrm{\θ}}$

$K_P\left(\mathrm{\θ}\right)={\stackrel{\‾}{K}}_P\tan \left({\stackrel{\‾}{\mathrm{\ω}}}_P\mathrm{\θ}\right)$

Wherein, K _P(θ) be non-linear P parameter, suitable selection parameter With

Value, controlled quentity controlled variable is existed | θ | in less, slope is less, at | θ | when larger, slope is larger, better carries out the robot balance and controls.

The fixed point balance.At small magnet 15 of motor 12 rear ends installations, when motor 12 rotated, small magnet 15 rotated thereupon, and the every induction region through a Hall element 14 of small magnet 15 can produce a pulse.The quantity that control chip 3 produces pulse by calculating can be calculated the displacement of robot.When orthostatic robot under external force, after having moved forward a distance x, Hall element 14 detects this section and moves, control chip 3 drive motors move identical apart from x to initial point, make robot get back to origin position, realize the fixed point balance.Its dominated formulate is:

$\mathrm{PD}(\mathrm{\θ},x)=\mathrm{PD}\left(\mathrm{\θ}\right)+K_{X}x+K_V\stackrel{\·}{x}$

Wherein, x=(x _L+ x _R)/2 are the left and right wheels average displacement.

Keep away barrier.Basis is novel carries out the induction of barrier by ultrasonic sensor 8, as shown in Figure 4, robot is in motion process, the transmitting terminal of ultrasonic sensor 8 is with forwards sector region emission of fixed frequency sound wave, and sound wave runs into the circular barrier back reflection of right front, after receiving terminal is received, pass to main control chip 3, main control chip 3 is controlled two motors 12 and is made corresponding action, and two wheel speed that make robot are different and reach turning, with avoiding obstacles.Its formula of using has:

${\mathrm{PD}}_L\left(\mathrm{\Δ}{\stackrel{\·}{x}}_D\right)=K_{\mathrm{\ψP}}({\stackrel{\·}{x}}_R-{\stackrel{\·}{x}}_L-\mathrm{\Δ}{\stackrel{\·}{x}}_D)+K_{\mathrm{\ψD}}({\stackrel{\·\·}{x}}_R-{\stackrel{\·\·}{x}}_L-\mathrm{\Δ}{\stackrel{\·\·}{x}}_D)$

${\mathrm{PD}}_L\left(\mathrm{\Δ}{\stackrel{\·}{x}}_D\right)=K_{\mathrm{\ψP}}({\stackrel{\·}{x}}_L-{\stackrel{\·}{x}}_R-\mathrm{\Δ}{\stackrel{\·}{x}}_D)+K_{\mathrm{\ψD}}({\stackrel{\·\·}{x}}_L-{\stackrel{\·\·}{x}}_R-\mathrm{\Δ}{\stackrel{\·\·}{x}}_D)$

Wherein,

Speed difference for the left and right wheels expectation.Definition is turned left counterclockwise) for being rotated in the forward,

Mean to be rotated in the forward, Mean reverse rotation, in order to guarantee robot pose balance, K _{ψ P}And K _{ψ D}Should be enough little.

4) tracking.Originally novel have the characteristics of different reflected intensitys by infrared sensor 13 infrared rays at the body surface of different colours, as shown in Figure 5, when infrared ray shines on atrament, infrared ray is substantially all by black absorption, seldom record reflection ray (Fig. 5), and when infrared ray found on whiteness, infrared ray substantially all was reflected back (Fig. 6).As shown in Figure 7, robot moves on the white background black line, and the result that the infrared sensor that robot right side robot motion's direction is as the criterion detects is black, but the result that the left side infrared sensor detects is white.Infrared sensor 14 is transferred to control chip 3 with testing result, and control chip 3 calculates rear drive motor 12 through algorithm, makes the speed of left side motor greater than the speed of the right motor, and robot comes back on black trace after moving right corresponding distance.

5) Remote.The JY-Bluetooth bluetooth module 6 of this novel employing can with all notebook and the bluetooth module pairing of mobile phone on the market now, its valid function distance is 10 meters.Straighforward operation makes robot make corresponding Motion to corresponding button, as press lower keyboard W key robot and travel forward, pressing S key robot moves backward, press A key robot to left movement, press D key robot and move right, press Q key robot and carry out original place spinfunction etc.

Claims (2)

1. desktop type Double-wheel self-balancing robot, it is characterized in that: main body rack (1), be fixed on two electric machine supports (11) of main body rack (1) below, be fixed in two motors (12) on two electric machine supports (11), be individually fixed in two wheels (9) on two motors (12), be fixed in the small magnet (15) of motor tail end, be fixed in upper layer bracket (2) on main body rack (1) by copper post (4), be fixed in the control chip (3) between main body rack (1) and upper layer bracket (2), be fixed in battery (5) and attitude transducer (10) on main body rack (1), be placed in the electric pressure converter (7) on upper layer bracket (2), ultrasonic sensor (8) and bluetooth module (6), and be fixed in Hall element (14) between two motors in main body rack (1) below (12), two infrared sensors (13) that also have two motors (12) the place ahead, battery (5) connects electric pressure converter (7), be main control chip (3) and motor-drive circuit power supply, the output of ultrasonic sensor (8) and infrared sensor (13) and Hall element (14) is connected with the input of main control chip (3) respectively, and the output of main control chip (3) is connected with motor (12).

2. a kind of desktop type two-wheel vertical type self balancing robot according to claim 1 is characterized in that: be achieved as follows function:

1) free balance; The attitude angle that attitude transducer (10) detection machine people is current transfers data to control chip (3), and control chip (3) draws the PWM ripple and controls motor (12) motion, makes robot keep balance;

2) fixed point balance; Keep under the robot equipoise, promote robot to last segment distance, Hall element (14) records robot motion's concrete displacement, and control chip (3) is controlled motor (12) and moved backward identical position, makes robot get back to origin position;

3) keep away barrier; Robot moves under poised state, ultrasonic sensor (8) is detection machine people the place ahead road conditions ceaselessly, when ultrasonic sensor (8) detects the place ahead barrier is arranged, control chip (3) is controlled respectively the speed of two motors (12), realize that robot turns, avoid the place ahead barrier, realize keeping away barrier;

4) tracking; Robot is in the process of motion, two infrared sensors (13) that are placed in the front lower place utilize infrared ray to have the characteristics of different reflected intensitys at the body surface of different colours, track is identified, control chip (3) is controlled motor (12) motion, makes robot along set track walking;

5) Remote; Bluetooth module (6) is installed, it and mobile phone and notebook Bluetooth pairing in robot.

Images