A balance control method for a dual gyro mechanism balance vehicle

Abstract

The invention discloses a balancing control method for a dual-gyro mechanism balancing vehicle, and relates to the technical field of balancing vehicles. The method comprises: obtaining the actual precession angle of the dual-gyro mechanism of the vehicle and making a difference with the expected precession angle, and obtaining the expected body inclination angle through PI loop control; obtaining the actual body inclination angle and making a difference with the expected body inclination angle, and obtaining the precession angle control speed of the dual-gyro mechanism through PID loop control as the control signal of the speed loop of the vehicle servo motor; the servo motor controls the deflection of the dual-gyro mechanism based on the control signal, thereby generating a corresponding correction torque to balance the body weight torque component; repeatedly obtaining the actual precession angle of the dual-gyro mechanism of the vehicle and making a difference with the expected precession angle, so that the actual precession angle is maintained near zero. The method ensures that the gyro precession angle is zero while ensuring the balance of the vehicle body, can effectively resist external interference, and ensure that the gyro effect in the dual-gyro scheme is always effective.

Description

Technical Field

[0001] The invention relates to the technical field of balancing vehicles, and in particular to a balancing control method for a balancing vehicle with a double gyro mechanism. Background Art

[0002] The non-coaxial two-wheeled robot is different from the coaxial two-wheeled robot on the market. It has the characteristics of narrow and long body and flexible movement. It is suitable for complex road conditions and narrow roads. The more representative product is the battery car. As we all know, the battery car is a typical under-driven system. It has the characteristics of tilting to the left or right without external force. Therefore, the battery car needs external force to achieve self-balancing. The fuzzy PID control method and sliding mode control method are currently widely used in the balance control algorithm of the dual gyro solution. These control algorithms only control the inclination angle of the vehicle body. In extreme cases, the gyro effect of the battery car will fail, causing the body to lose balance. Summary of the invention

[0003] In view of the above problems and technical requirements, the inventors have proposed a balance control method for a dual-gyro mechanism balancing vehicle, which adopts dual-loop PID control to ensure that the gyro precession angle is always maintained near zero and a large correction torque is generated, thereby ensuring the balance of the vehicle body, being able to effectively resist external interference, and ensuring that the gyro effect in the dual-gyro scheme is always effective.

[0004] The technical solution of the present invention is as follows:

[0005] A balance control method for a dual gyro mechanism balance vehicle comprises the following steps:

[0006] The actual precession angle of the vehicle's dual gyro mechanism is obtained and the expected precession angle is subtracted, and the expected vehicle body inclination angle is obtained through PI loop control;

[0007] The actual body inclination angle is obtained and the expected body inclination angle is subtracted. The precession angle control speed of the dual gyro mechanism is obtained through PID loop control, which is used as the control signal of the speed loop of the vehicle servo motor.

[0008] The servo motor controls the deflection of the dual gyro mechanism based on the control signal, thereby generating a corresponding correction torque to balance the weight torque component of the vehicle body;

[0009] The above process of obtaining the difference between the actual precession angle of the vehicle dual gyro mechanism and the expected precession angle is repeated to maintain the actual precession angle near zero.

[0010] A further technical solution is to obtain a desired vehicle body tilt angle through PI loop control, including:

[0011] The difference between the actual precession angle and the expected precession angle is processed by proportional terms and integral terms to obtain the unloading torque, which is expressed as:

[0012]

[0013] Among them, θ gc-m is the actual precession angle, θ gc-e is the desired precession angle, which is taken as zero, T unload is the unloading moment, J Aa-xx is the x component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point, k P0 , k I0 are proportional term parameter and integral term parameter respectively, h is the control period, n is the sampling point, is the angular velocity of the rotor of the dual gyro mechanism;

[0014] Substituting the desired precession angle state of the dual gyro mechanism and the estimated initial disturbance torque into the rolling channel dynamic equation, and considering the desired body inclination angle state, we have:

[0015]

[0016] in, is the expected body tilt angle; T d0 is the initial disturbance torque, which refers to the deviation of the actual body inclination angle from 0 due to the uneven distribution of vehicle weight; T ψ is the centrifugal torque related to the front wheel steering angle, T dψ is the centrifugal torque related to the front wheel steering angular velocity; ψ fs is the front wheel steering angle, is the vehicle speed, g is the acceleration due to gravity, S A,a-z is the first-order moment z component of the loaded vehicle relative to the rear wheel contact point, θ b is the pitch angle.

[0017] A further technical solution is to control the dual gyro mechanism to generate a corresponding correction torque based on the control signal, including:

[0018] Substitute the precession angle control speed of the dual gyro mechanism and the estimated initial disturbance torque into the rolling channel dynamic equation, consider the desired body inclination state, and obtain the correction torque, which is expressed as:

[0019]

[0020] Among them, T cmg is the correction torque, is the moment of inertia of the rotor of the dual gyro mechanism around the rotation axis, is the precession angle control speed of the dual gyro mechanism, is the angular velocity of the rotor of the dual gyro mechanism.

[0021] A further technical solution is to obtain the precession angle control speed of the dual gyro mechanism through PID loop control, including:

[0022] The difference between the expected body inclination angle at the nth sampling point and the actual body inclination angle at the nth sampling point is processed by proportional terms and integral terms, and the actual body inclination angle velocity at the nth sampling point is processed by differential terms to obtain the precession angle control velocity of the dual gyro mechanism, which is expressed as:

[0023]

[0024] in, is the precession angle control speed of the nth sampling point, J A,a-xx is the x component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point, are proportional term parameter, integral term parameter and differential term parameter respectively, h is the control period, is the expected body inclination angle at the nth sampling point, is the actual body inclination angle at the nth sampling point, T d-act-f It is the parameter adjustment torque, which is related to the vehicle body gravity torque. is the moment of inertia of the rotor of the dual gyro mechanism around the rotation axis, θ gc-m Actual precession angle.

[0025] A further technical solution is that the rolling channel dynamic equation is expressed as:

[0026]

[0027] The moments in the formula are:

[0028]

[0029]

[0030]

[0031] Among them, T g is the gravity moment, K f3 is a dimensionless constant, K f0 is the first-order moment z component of the loaded vehicle with equivalent front and rear wheels relative to the rear wheel contact point, J A,a-xz is the z component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point;

[0032] In the above formula:

[0033]

[0034]

[0035]

[0036] S A,a-x =-ma ·l rw-a , S A,a-z =-m a ·h rw-a ;

[0037] Among them, J b,b-xx is the x component of the moment of inertia of the loaded vehicle around its center of mass, m a is the mass of the loaded vehicle, h rw-a is the height of the center of mass of the loaded vehicle from the ground, l rw-a is the longitudinal distance from the center of mass of the loaded vehicle to the point where the rear wheels touch down. is the moment of inertia of the front wheel around the axis of rotation, is the moment of inertia of the rear wheel around the axis of rotation, r fw is the front wheel radius, r rw is the rear wheel radius, θ fs For the front fork angle.

[0038] A further technical solution is to obtain the actual precession angle of the dual gyro mechanism of the vehicle, including:

[0039] The absolute encoder of the DC reduction motor in the dual gyro mechanism is used to obtain the number of pulses per unit time, and then the actual precession angular velocity of the dual gyro mechanism is obtained by combining the MT speed measurement method. The actual precession angular velocity is filtered by a second-order Butterworth filter, and then integrated to obtain the actual precession angle of the dual gyro mechanism.

[0040] A further technical solution is to obtain the actual vehicle body inclination angle, including:

[0041] The vehicle body tilt sensor composed of an accelerometer and a gyroscope is used to obtain the actual vehicle body tilt velocity through a first-order Mahony filter algorithm, and then the actual vehicle body tilt angle is obtained by integration.

[0042] The beneficial technical effects of the present invention are:

[0043] The balancing control method adopts the combination of inner and outer loops, and uses two PID loops to control the body inclination angle and the gyro precession angle. The desired body inclination angle output by the outer loop is used as the input of the inner loop, and the output of the inner loop is the precession angle control speed. Based on the gyro effect, the high-speed rotating flywheel is rotated along the deflection axis perpendicular to the rotation axis, thereby generating a corresponding correction torque, which is used to balance the gravity torque component of the vehicle body. Compared with traditional PID control, the self-balancing method of the present application ensures that a sufficiently large correction torque is generated at any time, so that the precession angle of the gyroscope is maintained near zero, and the body inclination angle is also maintained near zero, avoiding the disappearance of the gyro effect when the gyro precession angle reaches 90°, which greatly improves the self-balancing and anti-interference ability of the electric vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a schematic diagram of the balance control method provided in this application. DETAILED DESCRIPTION

[0045] The specific implementation of the present invention will be further described below in conjunction with the accompanying drawings.

[0046] A control system for a dual-gyro mechanism balancing vehicle, comprising a balancing vehicle body, a controller, and a sensor module and an actuator connected thereto. The sensor module comprises a vehicle body tilt sensor composed of an accelerometer and a gyroscope, and an absolute encoder of a motor; the controller is implemented based on the TMS320F28335DSP model, and the balancing control method proposed in the present application is integrated therein; the actuator comprises a servo motor (speed loop), a synchronous pulley, and a dual-gyro structure, and the synchronous pulley enables the front and rear wheels to move synchronously when the dual-gyro structure deflects; the front wheel steering motor of the balancing vehicle is composed of a servo motor (position loop) and a synchronous pulley thereon, and the rear wheel hub motor works in the speed loop.

[0047] Based on the above control system, a balance control method of a dual gyro mechanism balance vehicle proposed in this application is introduced in detail. Figure 1 As shown, the following steps are included:

[0048] Step 1: Construct the rolling channel dynamics equation.

[0049] Considering the balance and stability of the two-wheeled robot during parking and low-speed driving, the expression of the rolling channel dynamic equation is established as follows:

[0050]

[0051] The moments in the formula are:

[0052]

[0053]

[0054]

[0055] in:

[0056]

[0057]

[0058]

[0059] S A,a-x =-m a ·l rw-a , S A,a-z =-m a ·h rw-a .

[0060] The definitions of the variables in the above formula are shown in Table 1.

[0061] Table 1 Variable list

[0062]

[0063]

[0064] Step 2: Obtain the actual precession angle of the vehicle's dual gyro mechanism, including:

[0065] The absolute encoder of the DC reduction motor in the dual gyro mechanism is used to obtain the number of pulses per unit time, and then the actual precession angular velocity of the dual gyro mechanism is obtained by combining the MT speed measurement method. The actual precession angular velocity is filtered by a second-order Butterworth filter, and then integrated to obtain the actual precession angle of the dual gyro mechanism.

[0066] Step 3: Subtract the actual precession angle from the expected precession angle and obtain the expected body inclination angle through PI loop control, including:

[0067] Step 31: The difference between the actual precession angle and the expected precession angle is processed by proportional terms and integral terms to obtain the unloading torque, which is expressed as:

[0068]

[0069] Among them, θ gc-m is the actual precession angle, θ gc-e is the desired precession angle, which is taken as zero, T unload is the unloading moment, k P0 , k I0 are proportional term parameters and integral term parameters respectively, and n is the sampling point. Optionally, let k P0 =0.8, k I0 =0.05.

[0070] Step 32: Set the desired precession angle state of the dual gyro mechanism (i.e. ) and the estimated initial disturbance torque are substituted into the rolling channel dynamic equation, considering the desired body inclination state (i.e. ), then:

[0071]

[0072] in, is the expected body tilt angle; T d0 is the initial disturbance torque, which refers to the deviation of the actual body inclination angle from 0 due to the uneven distribution of vehicle weight.

[0073] Step 4: Get the actual vehicle body tilt angle, including:

[0074] The actual vehicle body tilt angle velocity is obtained by using the vehicle body tilt angle sensor and the first-order Mahony filtering algorithm, and then the actual vehicle body tilt angle is obtained by integration.

[0075] Step 5: Subtract the actual vehicle body inclination angle from the desired vehicle body inclination angle obtained in step 3, and obtain the precession angle control speed of the dual gyro mechanism through PID loop control, which is used as the control signal of the speed loop of the vehicle servo motor.

[0076] The difference between the expected body inclination angle at the nth sampling point and the actual body inclination angle at the nth sampling point is processed by proportional terms and integral terms, and the actual body inclination angle velocity at the nth sampling point is processed by differential terms to obtain the precession angle control velocity of the dual gyro mechanism, which is expressed as:

[0077]

[0078] in, is the precession angle control speed of the nth sampling point, are proportional term parameter, integral term parameter and differential term parameter respectively. is the expected body inclination angle at the nth sampling point, is the actual body inclination angle at the nth sampling point, T d-act-f is the parameter adjustment torque, which is related to the vehicle body gravity torque, θ gc-m The actual precession angle. Optionally, set

[0079] Step 6: The servo motor controls the deflection of the dual gyro mechanism based on the control signal, thereby generating a corresponding correction torque to balance the vehicle body weight torque component.

[0080] Substitute the precession angle control speed of the dual gyro mechanism and the estimated initial disturbance torque into the rolling channel dynamic equation, consider the desired body inclination state, and obtain the correction torque, which is expressed as:

[0081]

[0082] Among them, T cmg is the correction torque, is the precession angle control speed of the dual gyro mechanism.

[0083] Step 7: Repeat steps 2-6 to make the actual precession angle approach the desired precession angle, that is, maintain it near zero, so that the body inclination angle is also maintained near zero.

[0084] The above balance control method adopts the combination of inner and outer loops, obtains the actual body inclination angle and gyro precession angle through the sensor module, and uses two PID loops to control the body inclination angle and gyro precession angle at the same time. The desired body inclination angle output by the outer loop is used as the input of the inner loop. The output of the inner loop is the precession angle control speed, which acts on the speed loop of the servo motor, drives the dual gyro structure to deflect through the transmission method of the synchronous belt, and generates a corresponding correction torque to balance the gravity torque component to achieve vehicle balance. It avoids the disappearance of the gyro effect when the gyro precession angle reaches 90°, which greatly improves the self-balancing and anti-interference ability of the battery vehicle.

[0085] The above is only a preferred embodiment of the present application, and the present invention is not limited to the above embodiments. It is understood that other improvements and changes directly derived or associated by those skilled in the art without departing from the spirit and concept of the present invention should be considered to be included in the protection scope of the present invention.

Claims

1. A balance control method for a dual gyro mechanism balance vehicle, characterized in that: The method comprises: The actual precession angle of the vehicle's dual gyro mechanism is obtained and the expected precession angle is subtracted, and the expected vehicle body inclination angle is obtained through PI loop control; Obtaining the difference between the actual vehicle body inclination angle and the desired vehicle body inclination angle, and obtaining the precession angle control speed of the dual gyro mechanism through PID loop control as a control signal of the speed loop of the vehicle servo motor; The servo motor controls the deflection of the dual gyro mechanism based on the control signal, thereby generating a corresponding correction torque to balance the vehicle body weight torque component; The above process of obtaining the difference between the actual precession angle of the vehicle dual gyro mechanism and the expected precession angle is repeated to maintain the actual precession angle near zero.

2. The balance control method according to claim 1, characterized in that: The desired vehicle body tilt angle is obtained through PI loop control, including: The difference between the actual precession angle and the expected precession angle is processed by proportional term and integral term to obtain the unloading torque, which is expressed as: Among them, θ gc-m is the actual precession angle, θ gc-e is the desired precession angle, which is zero, T unload is the unloading moment, J A,a-xx is the x component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point, k P0 , k I0 are proportional term parameter and integral term parameter respectively, h is the control period, n is the sampling point, is the angular velocity of the rotor of the dual gyro mechanism; Substituting the desired precession angle state and the estimated initial disturbance torque of the dual gyro mechanism into the rolling channel dynamic equation and considering the desired vehicle body inclination angle state, we have: in, is the desired vehicle body inclination angle; T d0 is the initial disturbance torque, which refers to the deviation of the actual body inclination angle from 0 due to the uneven distribution of vehicle weight; T ψ is the centrifugal torque related to the front wheel steering angle, T dψ is the centrifugal torque related to the front wheel steering angular velocity; ψ fs is the front wheel steering angle, is the vehicle speed, g is the acceleration due to gravity, S A,a-z is the first-order moment z component of the loaded vehicle relative to the rear wheel contact point, θ b is the pitch angle.

3. The balance control method according to claim 1, characterized in that: Controlling the dual gyro mechanism to generate a corresponding correction torque based on the control signal includes: The precession angle control speed of the dual gyro mechanism and the estimated initial disturbance torque T d0 Substituting into the rolling channel dynamics equation and considering the expected body inclination state, the correction torque is obtained, and the expression is: Among them, T cmg is the correction torque, is the moment of inertia of the rotor of the dual gyro mechanism about the rotation axis, is the precession angle control speed of the dual gyro mechanism, is the angular velocity of the rotor of the dual gyro mechanism.

4. The balance control method according to claim 1, characterized in that: The precession angle control speed of the dual gyro mechanism is obtained through PID loop control, including: The difference between the expected body inclination angle at the nth sampling point and the actual body inclination angle at the nth sampling point is processed by proportional terms and integral terms, and the actual body inclination angle velocity at the nth sampling point is processed by differential terms to obtain the precession angle control velocity of the dual gyro mechanism, which is expressed as: in, is the precession angle control speed of the nth sampling point, J A,a-xx is the x component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point, are proportional term parameter, integral term parameter and differential term parameter respectively, h is the control period, is the expected body inclination angle of the nth sampling point, is the actual vehicle body inclination angle at the nth sampling point, T d-act-f It is the parameter adjustment torque, which is related to the vehicle body gravity torque. is the moment of inertia of the rotor of the dual gyro mechanism around the rotation axis, θ gc-m The actual precession angle.

5. The balance control method according to claim 2 or 3, characterized in that: The expression of the rolling channel dynamic equation is: Among them, J A,a-xx is the x component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point; Indicates the body inclination angle, Represents the vehicle body inclination acceleration; ψ fs is the front wheel steering angle; is the vehicle speed, is the vehicle acceleration; T cmg is the correction torque; θ gc is the precession angle of the dual gyro mechanism, is the precession angular velocity of the dual gyro mechanism; The moments in the formula are: Among them, T g is the gravitational moment, g is the gravitational acceleration, S A,a-z is the first-order moment z component of the loaded vehicle relative to the rear wheel contact point, θ b is the pitch angle, K f3 is a dimensionless constant, K f0 is the first-order moment z component of the loaded vehicle with equivalent front and rear wheels relative to the rear wheel contact point, J A,a-xz is the z component of the moment of inertia of the loaded vehicle relative to the rear wheel contact point; In the above formula: S A,a-x =-m a ·l rw-a ,S A,a-z =-m a ·h rw-a ; Among them, J b,b-xx is the x component of the moment of inertia of the loaded vehicle around its center of mass, m a is the mass of the loaded vehicle, h rw-a is the height of the center of mass of the loaded vehicle from the ground, l rw-a is the longitudinal distance from the center of mass of the loaded vehicle to the point where the rear wheels touch down. is the moment of inertia of the front wheel around the axis of rotation, is the moment of inertia of the rear wheel around the axis of rotation, r fw is the front wheel radius, r rw is the rear wheel radius, θ fs is the fork angle, l rw-fw It is the longitudinal distance from the center of mass of the front wheel to the contact point of the rear wheel.

6. The balance control method according to claim 1, characterized in that: Get the actual precession angle of the vehicle's dual gyro mechanism, including: The absolute encoder of the DC reduction motor in the dual gyro mechanism is used to obtain the number of pulses per unit time, and then the actual precession angular velocity of the dual gyro mechanism is obtained in combination with the MT speed measurement method; the actual precession angular velocity is filtered by a second-order Butterworth filter, and then integrated to obtain the actual precession angle of the dual gyro mechanism.

7. The balance control method according to claim 1, characterized in that: Get the actual vehicle inclination angle, including: The vehicle body tilt sensor composed of an accelerometer and a gyroscope is used to obtain the actual vehicle body tilt velocity through a first-order Mahony filter algorithm, and then the actual vehicle body tilt angle is obtained by integration.

Images