CN103529850A - Control method of two-wheeled self-balance vehicle - Google Patents

Abstract

A control method of a two-wheeled self-balance vehicle comprises the steps as follows: (1), performing initialization: (2), reading values of a gyroscope, an accelerometer and a rotation angle sensor as well as the pulse number of an encoder respectively; (3), obtaining a vehicle body inclination, a handlebar turning angle, motor speeds and a vehicle speed; (4), then calculating PWM (pulse width modulation) values of vertical control, direction control and speed control respectively through a PID (proportion integration differentiation) control algorithm; (5), superposing the three PWM values together and outputting the three PWM values to left and right motors; (6), then sending data of the gyroscope, the accelerometer, the vehicle body inclination, a battery voltage, motor currents and the vehicle speed to an upper computer so as to monitor the operating status of the whole vehicle; (7), when the battery voltage is monitored to be smaller than a preset value, and the motor currents or the vehicle speed is monitored to be larger than the preset value through monitoring, turning on corresponding LED warning lights; and (8), when the vehicle body inclination is larger than a preset angle through monitoring when the vehicle body inclination is monitored to be larger than a preset angle, determining that the vehicle body falls down, stopping the operation and returning to an initializer. According to the control method, a more accurate operational method is adopted.

Description

Double-wheel self-balancing car control method

Technical field

The present invention relates to double-wheel self-balancing car, especially double-wheel self-balancing car control method.

Background technology

Self-balance electric vehicle self-poise operation principles is to be mainly based upon in the ultimate principle of a kind of being called as " dynamic stability " (Dynamic Stabilization), namely the self-poise ability of vehicle itself.With built-in accurate solid-state gyroscope (Solid-State Gyroscopes), judge the residing posture state of vehicle body, see through central microprocessor accurate and high speed and calculate after suitable instruction, CD-ROM drive motor is accomplished the effect of balance.Current balance car allows user arbitrarily to rock when riding, and the control system on it can automatically be carried out drive motor processing and be carried out balance correction; Due to the algorithm errors of existing control system, cause the time of drive motor balance correction longer, the risk that the Self-Balancing vehicle of this type is existed is in use larger.

Summary of the invention

For solving the problems of the technologies described above, technical scheme of the present invention is: a kind of double-wheel self-balancing car control method, comprises the following steps:

(1) double-wheel self-balancing car powers on after operation, and first system enters each initialize routine, comprise phaselocked loop initialization, AD conversion initialization, timer initialization, PWM initialization with and port initialization;

(2) after then time delay 1s waiting system is stablized, then read gyroscope, accelerometer and rotary angle transmitter initial value, then time delay 1s waits for that parameter changes;

(3) program setting 1ms interrupt function, uprightly controlling, direction is controlled and speed is controlled during being all placed on and carried out simultaneously; When 1ms interrupts producing, read respectively the value of gyroscope, accelerometer and rotary angle transmitter and the pulse number of scrambler;

(4) by Kalman filtering, carry out gyroscope and accelerometer data fused filtering, ask for car body inclination angle; Value by rotary angle transmitter is calculated handlebar corner; Pulse number by scrambler calculates motor speed and the speed of a motor vehicle;

(5) then by pid control algorithm, calculate respectively the PWM value that upright control, direction control and speed are controlled;

(6) three PWM values are superimposed and export to left and right motor;

(7) data that then send gyroscope, accelerometer, car body inclination angle, cell voltage, current of electric and the speed of a motor vehicle are to host computer, to monitor the running status of car load;

(8) when monitoring cell voltage and be greater than preset value lower than preset value, current of electric or the speed of a motor vehicle, open corresponding LED warning lamp; When monitoring car body inclination angle and be greater than preset angles, be just judged to be car body and fall, and out of service, come back in the middle of initialize routine.

The beneficial effect that the present invention compared with prior art brought is:

The present invention adopts more accurate operational method, makes the time of drive motor balance correction shorter, reduces the risk that Self-Balancing vehicle exists in use.

Accompanying drawing explanation

Fig. 1 is control flow chart of the present invention.

Embodiment

Below in conjunction with Figure of description, the invention will be further described.

As shown in Figure 1, a kind of double-wheel self-balancing car control method, comprises the following steps:

(1) double-wheel self-balancing car powers on after operation, and first system enters each initialize routine, comprise phaselocked loop initialization, AD conversion initialization, timer initialization, PWM initialization with and port initialization;

(2) after then time delay 1s waiting system is stablized, then read gyroscope, accelerometer and rotary angle transmitter initial value, then time delay 1s waits for that parameter changes;

(3) program setting 1ms interrupt function, uprightly controlling, direction is controlled and speed is controlled during being all placed on and carried out simultaneously; When 1ms interrupts producing, read respectively the value of gyroscope, accelerometer and rotary angle transmitter and the pulse number of scrambler;

(4) by Kalman filtering, carry out gyroscope and accelerometer data fused filtering, ask for car body inclination angle; Value by rotary angle transmitter is calculated handlebar corner; Pulse number by scrambler calculates motor speed and the speed of a motor vehicle;

(5) then by pid control algorithm, calculate respectively the PWM value that upright control, direction control and speed are controlled;

(6) three PWM values are superimposed and export to left and right motor;

(7) data that then send gyroscope, accelerometer, car body inclination angle, cell voltage, current of electric and the speed of a motor vehicle are to host computer, to monitor the running status of car load;

(8) when monitoring cell voltage and be greater than preset value lower than preset value, current of electric or the speed of a motor vehicle, open corresponding LED warning lamp; When monitoring car body inclination angle and be greater than preset angles, be just judged to be car body and fall, and out of service, come back in the middle of initialize routine.

Correlation value calculation aa+=P_enc*enc_anglespeed*dt*0.001+ (1-P_enc) * (acc_angle-angle); Calculate at // inclination angle;

Aa: the inclination angle that gyroscope and accelerometer draw after merging;

P_enc: scale-up factor;

Enc_angkespeed: gyrostatic AD value;

Dt: gyroscope integral time;

Acc_angle: angular velocity AD value;

Angle: inclination angle.

PWM_angle=(int) (pp*angle+D_angle*enc_anglespeed); // upright PWM calculates;

PP: scale-up factor;

D_angle: differential coefficient;

PWM_angle: upright PWM value.

PWM_turn=(int) (P_turn*turn); // turn to PWM to calculate;

P_turn: ratio of turning coefficient;

Turn: the AD value of rotation direction sensor;

PWM_turn; Turn to PWM value.

1, each parts parameter designing and calculating of double-wheel self-balancing car

The single shaft electric motor car parameter designing of riding instead of walk is mainly to determine that general structure size, mass parameter, Specifeca tion speeification, motor nominal parameter, ratio of gear size, battery capacity are big or small etc.

1.1, complete vehicle structure parameter

(1) contour dimension

What scooter was focused on is portable, dirigibility, and car body is similar at ground projected area and people's shoulder breadth.Therefore, just fixed whole contour dimension is long 680mm, wide 360mm, and high 1140mm, wherein step height is 250mm.

(2) wheelspan

For scooter, wheelspan change main with have influence on roll stiffness.Increase wheelspan, be conducive to increase roll stiffness, lateral stability is good, but what follow is the increase of car load length and quality.Therefore, considering rear selection wheelspan is 600mm.

(3) mass parameter

Scooter quality accounting maximum be lithium battery group, motor, speed reduction unit and wheel, quality is larger, power consumption is more, thereby reduces the mileage that cruises.In order to control mass value, body part has adopted truss structure, and pedal and car shell adopt carbon fiber structural, guarantee that the while attractive in appearance alleviates complete vehicle quality to greatest extent.

Because the detailed quality of each parts is not known, complete vehicle curb weight cannot accurately obtain.But for from energy-conserving and environment-protective, flexible light, complete vehicle curb weight is just elected 40kg as, and mounted mass is 100kg.(4) Specifeca tion speeification

According to investigation, general scooter max. speed is 15-25km/h, and to accelerate to the max. speed time used be 2-5s from static, and the highest gradient that can climb up is 15-30 °; Therefore determine that the single shaft electric motor car dynamic property parameter of riding instead of walk is: max. speed 20km/h, acceleration time 3s, 20 ° of max. climb slope.Because scooter is as short distance running vehicle, the speed of a motor vehicle is unsuitable too high, otherwise easily causes instability, reduces safety coefficient; Acceleration time is at mild cement pavement, to accelerate to continuously the max. speed time used under the inclination angle allowing at car body, and scooter is quick on the draw, general with seldom the time just can reach high-speed cruising state; Scooter is that level road use is more, and occasional runs into upward slope situation, thereby sets 20 ° of maximum climbing attitudes, to meet the demand of different crowd.

Because scooter left and right motor all can independently be controlled, by control system, realize and turn to differential control, so minimal curve radius is zero, maneuverability.

1.2, the parameter of electric machine

All actions of scooter are all to be completed by motor, and the parameter of electric machine is determined very crucial, must select suitable motor to meet car load request for utilization.Power of motor is larger, and dynamic property is better, but is also accompanied by mass penalty, volume increase, price increase.Therefore, motor parameters key is to determine the rated power of motor.

Scooter is herein declared working condition while take max. speed operation, power demand:

$P=\frac{1}{\mathrm{\η}}(\frac{G{\mathrm{fu}}_a}{3600}+\frac{C_D{\mathrm{Au}}_a^3}{76140})$

（2-1）

Wherein, η is transmission efficiency, and G is the gravity (N) acting on automobile, and f is coefficient of rolling resistance, u _afor max. speed (km/h), C _dfor coefficient of air resistance; A is front face area (m ²): when scooter is unloaded, G=40 * 9.8N, f=0.012, C _d=0.3, A=1.2m ², u _a=20km/h, η=0.95

$P=\frac{1}{0.95}(\frac{40\×9.8\×0.012\×20}{3600}+\frac{0.3\×1.2\×{20}^3}{76140})=0.094\mathrm{kW}$

Scooter full load, G=140 * 9.8N, f=0.012, C _d=0.3, A=2m ², u _a=20km/h, η=0.95

$P=\frac{1}{0.95}(\frac{140\×9.8\×0.012\×20}{3600}+\frac{0.3\×2\×{20}^3}{76140})=0.163\mathrm{kW}$

But scooter can both keep the key of balance to be that the stand-by power of motor is enough large at any time, empirical evidence, the rated power of motor must be to calculate the more than 3 times of power demand.Therefore, selected direct current generator major parameter is herein: rated voltage 24V, rated power 500W, rated speed 4500r/min, nominal torque 1.06Nm.

1.3, ratio of gear

By relation between car load max. speed and motor rated speed, determine ratio of gear.Tire outside diameter is 14in herein, tables look-up and can obtain vehicle wheel roll radius r=180mm, max. speed v=20km/h=333.3m/min, wheel circumference C=2 π r=2 * 3.14 * 0.18=1.13m(2-2)

Wheel rated speed=

(2-3)

Ratio of gear $i=\frac{4500}{294.9}\≈15$ （2-4）

1.4, lithium ion battery

Lithium ion battery parameter determines that key is battery capacity size, can obtain by the car load mileage calculation that cruises.Because various factorss such as being subject to temperature, humidity, battery material, service condition affects, the impossible full capacity of battery, more impossible electric discharge completely, will quit work to certain voltage.Therefore, set battery capacity availability coefficient λ=0.8 during calculating.

When scooter moves in max. speed, electric current

(2-5)

The mileage that cruises is 35km required time (2-6)

Battery capacity is $Q=\frac{\mathrm{It}}{\mathrm{\λ}}=\frac{6.79\×1.75}{0.8}=14.85\mathrm{Ah}$ （2-7）

Therefore, select the lithium battery group of 24V15Ah series connection assembling.

2, dynamic property is checked

2.1, the acceleration time checks

Calculate below under rated voltage, scooter is from the static required time t of max. speed that accelerates to, and derivation is as follows in detail:

Motor speed

(2-8)

Motor output torque

(2-9)

Wheel drive torque

(2-10)

Can obtain car load acceleration:

$a=\frac{\mathrm{du}}{\mathrm{dt}}=\frac{F}{m}=\frac{T_t/r-\mathrm{Gf}}{m}=\frac{318.3\frac{P_N\mathrm{\π\η}}{u}-\mathrm{mgf}}{m}$ （2-11）

Arrange:

$\frac{\mathrm{du}}{\mathrm{dt}}=\frac{318.3\×3.14\×0.5\×2\×0.95}{140u}-0.012\×9.8=\frac{6.78}{u}-0.12$ （2-12）

Above formula both sides while integration is obtained:

$\∫\mathrm{dt}=\∫\frac{u}{6.78-0.12u}\mathrm{du}$ （2-13）

That is:

$t={\∫}_0^{5.6}\frac{u}{6.78-0.12u}\mathrm{du}$ （2-14）

Order $m=\frac{u}{6.78-0.12u},$ ? $u=\frac{6.78-m}{m},\mathrm{du}=-8.33\mathrm{dm}$

$\begin{array}{c}t=8.33{\∫}_{6.1}^{6.8}\frac{6.78-m}{0.12m}\mathrm{dm}\\ =8.33\×(56.5\ln m-8.33m{|}_{6.1}^{6.8})\\ =2.4s\end{array}$ （2-15）

As can be seen here, the single shaft electric motor car of riding instead of walk of design herein, at the dry cement pavement of level, starts to accelerate to max. speed 20km/h from static, and the acceleration time is 2.4s, is less than preset value 3s, and dynamic property is good.

2.2, max. climb slope is checked

When scooter goes up a slope, mainly stressed is resistance to rolling, grade resistance and driving force effect, and wherein resistance to rolling is F _f=Gfcos α (2-16)

Grade resistance is F _i=Gsin α (2-17)

Motor nominal torque $T_N=9550\frac{P_N}{n_N}=9550\frac{0.5}{4500}=1.06\mathrm{Nm}$ （2-18）

Motor maximum torque T _max=λ T _n(2-19)

Wherein, λ is overload factor, is generally 1.8-2.2, gets λ=2;

T _max=λT _N=2×1.06=2.12Nm

Owing to asking max. climb slope, scooter at the uniform velocity advances, by stress balance, obtained:

F _t=F _f+F _i（2-20）

Be G (fcos α+sin α)=F _t(2-21)

Arrange:

$\cos \mathrm{\α}+50\sin \mathrm{\α}=\frac{335.7\×50}{140\×9.8}=12.2$ （2-22）

$\frac{\cos \mathrm{\α}}{\sqrt{1+{50}^2}}+\frac{50\sin \mathrm{\α}}{\sqrt{1+{50}^2}}=\frac{12.2}{\sqrt{1+{50}^2}}$ （2-23）

Order $\sin \mathrm{\β}=\frac{1}{\sqrt{1+{50}^2}},\cos \mathrm{\β}\frac{50}{\sqrt{1+{50}^2}},$ :

$\sin \mathrm{\β}\cos \mathrm{\α}+\cos \mathrm{\β}\sin \mathrm{\α}=\frac{12.2}{\sqrt{1+{50}^2}}$ （2-24）

$\sin (\mathrm{\α}+\mathrm{\β})=\frac{12.2}{\sqrt{1+{50}^2}}$ （2-25）

Solve $\mathrm{\α}+\mathrm{\β}=\arcsin \left(\frac{12.2}{\sqrt{1+{50}^2}}\right)=25.3.$

$\mathrm{\β}=\arcsin \left(\frac{1}{\sqrt{1+{50}^2}}\right)=1.15.$

Max. climb slope α=alpha+beta-β=25.3-1.15=24.15 ° (2-26)

Therefore, max. climb slope is larger than 20 ° of preset values, meets the demands.

Claims (1)

1. a double-wheel self-balancing car control method, is characterized in that, comprises the following steps:

(1) double-wheel self-balancing car powers on after operation, and first system enters each initialize routine, comprise phaselocked loop initialization, AD conversion initialization, timer initialization, PWM initialization with and port initialization;

(2) after then time delay 1s waiting system is stablized, then read gyroscope, accelerometer and rotary angle transmitter initial value, then time delay 1s waits for that parameter changes;

(3) program setting 1ms interrupt function, uprightly controlling, direction is controlled and speed is controlled during being all placed on and carried out simultaneously; When 1ms interrupts producing, read respectively the value of gyroscope, accelerometer and rotary angle transmitter and the pulse number of scrambler;

(4) by Kalman filtering, carry out gyroscope and accelerometer data fused filtering, ask for car body inclination angle; Value by rotary angle transmitter is calculated handlebar corner; Pulse number by scrambler calculates motor speed and the speed of a motor vehicle;

(5) then by pid control algorithm, calculate respectively the PWM value that upright control, direction control and speed are controlled;

(6) three PWM values are superimposed and export to left and right motor;

(7) data that then send gyroscope, accelerometer, car body inclination angle, cell voltage, current of electric and the speed of a motor vehicle are to host computer, to monitor the running status of car load;

(8) when monitoring cell voltage and be greater than preset value lower than preset value, current of electric or the speed of a motor vehicle, open corresponding LED warning lamp; When monitoring car body inclination angle and be greater than preset angles, be just judged to be car body and fall, and out of service, come back in the middle of initialize routine.

Images