Method and device for controlling equipment balance
阅读说明：本技术 控制设备平衡的方法和装置 (Method and device for controlling equipment balance ) 是由 张洋 张虎 于 2020-06-29 设计创作，主要内容包括：本申请公开了一种控制设备平衡的方法和装置。所述方法的包括：获取姿态传感器测量的设备的加速度、角速度；利用加速度、角速度,进行姿态解算,计算设备的倾斜角；获取飞轮的当前转速；对角速度进行微分D控制,对倾斜角进行比例P控制,对飞轮的当前转速进行比例积分PI控制,用上述角速度的微分控制、倾斜角的比例控制、飞轮转速的比例积分控制的和,调制PWM信号；采用该PWM信号驱动电机带动飞轮转动,产生力矩,维持设备的平衡。实现了通过角速度微分控制,提前防治设备的倾斜,通过倾斜角比例控制,快速调整改善设备的倾斜状况,维持设备的平衡。此外,对飞轮的当前转速进行PI控制,快速降低飞轮的转速,同时消除飞轮惯性旋转累积的转速。(The application discloses a method and a device for controlling equipment balance. The method comprises the following steps: acquiring acceleration and angular speed of equipment measured by an attitude sensor; carrying out attitude calculation by utilizing the acceleration and the angular speed, and calculating the inclination angle of the equipment; acquiring the current rotating speed of a flywheel; carrying out differential D control on angular speed, carrying out proportional P control on an inclination angle, carrying out proportional integral PI control on the current rotating speed of the flywheel, and modulating a PWM signal by using the sum of the differential control on the angular speed, the proportional control on the inclination angle and the proportional integral control on the rotating speed of the flywheel; the PWM signal is adopted to drive the motor to drive the flywheel to rotate, so that torque is generated, and the balance of equipment is maintained. The control method realizes the control of the inclination of the equipment in advance through angular velocity differential control, quickly adjusts and improves the inclination condition of the equipment through inclination angle proportional control, and maintains the balance of the equipment. In addition, the current rotating speed of the flywheel is subjected to PI control, so that the rotating speed of the flywheel is rapidly reduced, and the rotating speed accumulated by inertial rotation of the flywheel is eliminated.)
1. A method of controlling equipment balancing, the method comprising:
acquiring acceleration and angular speed of equipment measured by an attitude sensor;
carrying out attitude calculation by utilizing the acceleration and the angular speed, and calculating the inclination angle of the equipment;
acquiring the current rotating speed of a flywheel;
carrying out differential D control on the angular speed, carrying out proportional P control on the inclination angle, carrying out proportional integral PI control on the current rotating speed of the flywheel, and modulating a Pulse Width Modulation (PWM) signal by using the sum of the differential D control on the angular speed, the proportional P control on the inclination angle and the proportional integral PI control on the rotating speed of the flywheel;
and driving a motor to drive a flywheel to rotate by adopting the PWM signal, generating torque and maintaining the balance of the equipment.
2. The method according to claim 1, wherein said differentiating D control of said angular velocity, proportional P control of said tilt angle, proportional integral PI control of said flywheel current rotational speed, modulating a pulse width modulation PWM signal with the sum of said differentiating D control of angular velocity, proportional P control of tilt angle, proportional integral PI control of flywheel rotational speed, comprises:
differentiating the angular velocity of the device, and then multiplying by a differential gain to obtain a differential D control of the angular velocity;
setting an angle set value of the equipment, subtracting the angle set value from the inclination angle to obtain an angle deviation, and multiplying a first proportional gain by the angle deviation to obtain a proportional P control of the inclination angle;
setting a rotating speed set value of the flywheel, subtracting the rotating speed set value from the current rotating speed of the flywheel to obtain a rotating speed deviation, multiplying a second proportional gain by the rotating speed deviation to obtain a proportional P control of the rotating speed of the flywheel, integrating the rotating speed deviation, and multiplying the integral P control by an integral gain to obtain an integral I control of the rotating speed of the flywheel;
and calculating the sum of the differential D control of the angular speed, the proportional P control of the inclination angle, the proportional P control of the flywheel rotating speed and the integral I control, and modulating a Pulse Width Modulation (PWM) signal.
3. The method of claim 2, further comprising:
limiting the magnitude of the integral I control of the flywheel rotational speed;
limiting the magnitude of the sum of the derivative of angular velocity D control, the proportional of inclination angle P control, the proportional of flywheel speed P control, and the integral after clipping I control.
4. A method according to any of claims 2-3, characterized in that the rotational speed set point of the flywheel is zero.
5. The method according to any one of claims 1-3, further comprising:
and forming a Bluetooth mesh network with other modules and/or an upper computer of the equipment to perform Bluetooth communication.
6. The method of claim 5, further comprising:
and receiving and storing the adjusted PID parameters sent by the upper computer through the Bluetooth mesh network.
7. The method according to any one of claims 1-3, further comprising:
calculating an angular acceleration of the flywheel when the moment of the device is equal to the moment of the flywheel;
and adjusting PID parameters according to the angular acceleration of the flywheel and the balance condition of the equipment.
8. The method of claim 7, further comprising:
the torque of the device and the torque of the flywheel are calculated by the following formula:
T1＝M×g×cosθ×L
wherein, T1The moment of the equipment, M is the mass of the equipment, g is the gravity acceleration, L is the distance from the gravity center of the equipment to a contact point of the equipment and the ground, and theta is the included angle between the connection line of the gravity center of the equipment and the contact point of the equipment and the ground;
wherein, T2Is the moment of the flywheel, m is the mass of the flywheel, R is the outer radius of the flywheel, R is the inner radius of the flywheel, α is the angular acceleration of the flywheel;
when the moment of the device is equal to the moment of the flywheel, the angular acceleration α of the flywheel is:
9. an apparatus for controlling equipment balance, the apparatus comprising:
the attitude parameter acquisition unit is used for acquiring the acceleration and the angular speed of the equipment measured by the attitude sensor;
the attitude calculation unit is configured to use the acceleration and the angular velocity to calculate an attitude and calculate an inclination angle of the equipment;
the flywheel parameter obtaining unit is configured for obtaining the current rotating speed of the flywheel;
a PID control unit configured to perform differential D control on the angular velocity, perform proportional P control on the tilt angle, perform proportional integral PI control on the current rotation speed of the flywheel, and modulate a Pulse Width Modulation (PWM) signal by the sum of the differential D control on the angular velocity, the proportional P control on the tilt angle, and the proportional integral PI control on the rotation speed of the flywheel;
and the driving unit is configured to drive the motor to drive the flywheel to rotate by adopting the PWM signal, so that torque is generated, and the balance of the equipment is maintained.
10. The apparatus of claim 9, wherein the PID control unit comprises:
the angular velocity control subunit is configured to differentiate the angular velocity of the device and then multiply the angular velocity by a differential gain to obtain a differential D control of the angular velocity;
an angle control subunit, configured to set an angle setting value of the device, subtract the angle setting value from the tilt angle to obtain an angle deviation, and multiply the angle deviation by a first proportional gain to obtain a proportional pcontrol of the tilt angle;
the flywheel rotation speed control subunit is configured to set a rotation speed set value of the flywheel, subtract the rotation speed set value from the current rotation speed of the flywheel to obtain a rotation speed deviation, multiply a second proportional gain by the rotation speed deviation to obtain a proportional P control of the rotation speed of the flywheel, integrate the rotation speed deviation, and multiply by an integral gain to obtain an integral I control of the rotation speed of the flywheel;
a modulation subunit configured to calculate a sum of a derivative of the angular velocity, Dcontrol, a proportional of the tilt angle, Pcontrol, a proportional of the flywheel rotational speed, Pcontrol, and an integral, Icontrol, for modulating the pulse width modulated PWM signal.
11. The apparatus of claim 9, further comprising:
and the Bluetooth communication unit is configured to form a Bluetooth mesh network with other modules and/or an upper unit of the equipment to perform Bluetooth communication.
12. The apparatus of any of claims 9-11, further comprising:
an angular acceleration calculation unit configured to calculate an angular acceleration of a flywheel when a moment of the apparatus is equal to a moment of the flywheel;
and the PID parameter adjusting unit is configured to adjust PID parameters according to the angular acceleration of the flywheel and the balance condition of the equipment.
Technical Field
The application relates to the technical field of control, in particular to a method and a device for controlling equipment balance.
Background
Single wheel, two-wheeled equipment, robot, telecar have extremely strong flexibility, but these equipment equilibrium nature is poor, often appear about and/or the front and back slope's difficult problem.
Disclosure of Invention
The object of the present application is to propose an improved method and apparatus for controlling the balancing of a device, solving the technical problems mentioned in the background section above.
In a first aspect, the present application provides a method of controlling equipment balancing, the method comprising: acquiring acceleration and angular speed of equipment measured by an attitude sensor; carrying out attitude calculation by utilizing the acceleration and the angular speed, and calculating the inclination angle of the equipment; acquiring the current rotating speed of a flywheel; carrying out differential D control on the angular speed, carrying out proportional P control on the inclination angle, carrying out proportional integral PI control on the current rotating speed of the flywheel, and modulating a Pulse Width Modulation (PWM) signal by using the sum of the differential D control on the angular speed, the proportional P control on the inclination angle and the proportional integral PI control on the rotating speed of the flywheel; and driving a motor to drive a flywheel to rotate by adopting the PWM signal, generating torque and maintaining the balance of the equipment.
In some embodiments, the differentiating D control of the angular velocity, the proportional P control of the tilt angle, and the proportional integral PI control of the current rotation speed of the flywheel, and the modulating the PWM signal with the sum of the differentiating D control of the angular velocity, the proportional P control of the tilt angle, and the proportional integral PI control of the rotation speed of the flywheel, comprises: differentiating the angular velocity of the device, and then multiplying by a differential gain to obtain a differential D control of the angular velocity; setting an angle set value of the equipment, subtracting the angle set value from the inclination angle to obtain an angle deviation, and multiplying a first proportional gain by the angle deviation to obtain a proportional P control of the inclination angle; setting a rotating speed set value of the flywheel, subtracting the rotating speed set value from the current rotating speed of the flywheel to obtain a rotating speed deviation, multiplying a second proportional gain by the rotating speed deviation to obtain a proportional P control of the rotating speed of the flywheel, integrating the rotating speed deviation, and multiplying the integral P control by an integral gain to obtain an integral I control of the rotating speed of the flywheel; and calculating the sum of the differential D control of the angular speed, the proportional P control of the inclination angle, the proportional P control of the flywheel rotating speed and the integral I control, and modulating a Pulse Width Modulation (PWM) signal.
In some embodiments, the method further comprises: limiting the magnitude of the integral I control of the flywheel rotational speed; limiting the magnitude of the sum of the derivative of angular velocity D control, the proportional of inclination angle P control, the proportional of flywheel speed P control, and the integral after clipping I control.
In some embodiments, the flywheel has a speed set point of zero.
In some embodiments, the method further comprises: and forming a Bluetooth mesh network with other modules and/or an upper computer of the equipment to perform Bluetooth communication.
In some embodiments, the method further comprises: and receiving and storing the adjusted PID parameters sent by the upper computer through the Bluetooth mesh network.
In some embodiments, the method further comprises: calculating an angular acceleration of the flywheel when the moment of the device is equal to the moment of the flywheel; and adjusting PID parameters according to the angular acceleration of the flywheel and the balance condition of the equipment.
In some embodiments, the method further comprises: the torque of the device and the torque of the flywheel are calculated by the following formula:
T1＝M×g×cosθ×L
wherein, T1The moment of the equipment, M is the mass of the equipment, g is the gravity acceleration, L is the distance from the gravity center of the equipment to a contact point of the equipment and the ground, and theta is the included angle between the connection line of the gravity center of the equipment and the contact point of the equipment and the ground;
wherein, T2Is the moment of the flywheel, m is the mass of the flywheel, R is the outer radius of the flywheel, R is the inner radius of the flywheel, α is the angular acceleration of the flywheel;
when the moment of the device is equal to the moment of the flywheel, the angular acceleration α of the flywheel is:
in a second aspect, the present application provides an apparatus for controlling the balancing of a device, the apparatus comprising: the attitude parameter acquisition unit is used for acquiring the acceleration and the angular speed of the equipment measured by the attitude sensor; the attitude calculation unit is configured to use the acceleration and the angular velocity to calculate an attitude and calculate an inclination angle of the equipment; the flywheel parameter obtaining unit is configured for obtaining the current rotating speed of the flywheel; a PID control unit configured to perform differential D control on the angular velocity, perform proportional P control on the tilt angle, perform proportional integral PI control on the current rotation speed of the flywheel, and modulate a Pulse Width Modulation (PWM) signal by the sum of the differential D control on the angular velocity, the proportional P control on the tilt angle, and the proportional integral PI control on the rotation speed of the flywheel; and the driving unit is configured to drive the motor to drive the flywheel to rotate by adopting the PWM signal, so that torque is generated, and the balance of the equipment is maintained.
In some embodiments, the PID control unit includes: the angular velocity control subunit is configured to differentiate the angular velocity of the device and then multiply the angular velocity by a differential gain to obtain a differential D control of the angular velocity; an angle control subunit, configured to set an angle setting value of the device, subtract the angle setting value from the tilt angle to obtain an angle deviation, and multiply the angle deviation by a first proportional gain to obtain a proportional pcontrol of the tilt angle; the flywheel rotation speed control subunit is configured to set a rotation speed set value of the flywheel, subtract the rotation speed set value from the current rotation speed of the flywheel to obtain a rotation speed deviation, multiply a second proportional gain by the rotation speed deviation to obtain a proportional P control of the rotation speed of the flywheel, integrate the rotation speed deviation, and multiply by an integral gain to obtain an integral I control of the rotation speed of the flywheel; a modulation subunit configured to calculate a sum of a derivative of the angular velocity, Dcontrol, a proportional of the tilt angle, Pcontrol, a proportional of the flywheel rotational speed, Pcontrol, and an integral, Icontrol, for modulating the pulse width modulated PWM signal.
In some embodiments, the apparatus further comprises: and the Bluetooth communication unit is configured to form a Bluetooth mesh network with other modules and/or an upper unit of the equipment to perform Bluetooth communication. In some embodiments, the apparatus further comprises: an angular acceleration calculation unit configured to calculate an angular acceleration of a flywheel when a moment of the apparatus is equal to a moment of the flywheel; and the PID parameter adjusting unit is configured to adjust PID parameters according to the angular acceleration of the flywheel and the balance condition of the equipment.
The method adopts a PID control algorithm to control the balance of the equipment, firstly obtains the acceleration and the angular velocity of the equipment, calculates the inclination angle of the equipment, and carries out proportion P control on the inclination angle of the equipment, and the proportion P control can be used for quickly adjusting and improving the inclination condition of the equipment. The differential D control is carried out on the angular speed of the device, and the adjustment is started once the inclination angle generation trend is found before the inclination angle is generated, so that the control is carried out in advance and is more timely. When the angular speed tends to become larger or smaller, the output control is performed to prevent and control the inclination angle from becoming larger or overshooting in advance. Through the inclination angle proportion P control and the angular speed differential D control, the equipment can oscillate near a balance point, but the flywheel rotates faster, so that the proportion integral PI control is carried out on the current rotating speed of the flywheel, the rotating speed of the flywheel is rapidly reduced, and the rotating speed accumulated by the inertial rotation of the flywheel is eliminated. The device is prevented from inclining in advance while keeping the balance of the device, the inclination angle is continuously reduced, the device returns to a natural balance state, and the rotation speed of the flywheel is controlled to be reduced.
Drawings
Other features, objects and advantages of the present application will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, made with reference to the accompanying drawings in which:
FIG. 1 is a schematic view of a flywheel electric machine according to an embodiment of the present application;
FIG. 2 is a flow chart of one embodiment of a method of controlling device balancing of the present application;
FIG. 3 is a flow chart of a PID control algorithm in one embodiment of a method of controlling plant balance of the present application;
FIG. 4 is a schematic diagram illustrating a method for controlling equipment balancing according to the present application;
fig. 5 is a schematic structural diagram of an embodiment of the apparatus for controlling the balance of the device.
Detailed Description
The present application will be described in further detail with reference to the following drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the relevant invention and not restrictive of the invention. It should be noted that, for convenience of description, only the portions related to the related invention are shown in the drawings.
It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict. The present application will be described in detail below with reference to the embodiments with reference to the attached drawings.
The device utilizes the flywheel precession principle to control the angular acceleration of the flywheel, so that the device generates reverse action torque to control the balance of the device. Wherein the flywheel motor is mounted on the device. Referring to fig. 1, a schematic diagram of a flywheel motor according to an embodiment is shown. As shown in the figure, the flywheel motor includes a micro control unit MCU101, a
With continuing reference to FIG. 2, which is a flow chart of one embodiment of a method of controlling device balancing according to the present application, the method includes the steps of:
step 201, acquiring acceleration and angular velocity of the equipment measured by the attitude sensor.
The embodiment is used for controlling the balance of the longitudinal two-wheeled equipment. The longitudinal two-wheeled device includes but is not limited to: longitudinal two-wheeled robots, electric motorcycles, remote control toy motorcycles, bicycles ridden by users, and remote control toy bicycles. Wherein, the installation axis of the flywheel is along the driving direction of the vehicle body. For example, the flywheel is mounted under the seat of an electric motorcycle, and the axis of rotation of the flywheel is also the longitudinal front-rear direction.
In this embodiment, the attitude sensor is a six-axis gyroscope, and the six-axis gyroscope can measure three-axis acceleration and three-axis angular velocity of the device. In other optional implementation manners of this embodiment, the attitude sensor is a three-axis gyroscope and an accelerometer, and is configured to measure a three-axis angular velocity and a three-axis acceleration of the device, respectively. During installation, the direction of the attitude sensor is adjusted, one axis of the attitude sensor is the longitudinal direction of the equipment, and the other axis of the attitude sensor is the left and right transverse directions of the equipment. The left and right inclination angles of the equipment are the pitch angle or roll angle of the equipment.
And 202, carrying out attitude calculation by using the acceleration and the angular velocity, and calculating the inclination angle of the equipment.
In this embodiment, sliding window filtering is performed on the acceleration. And performing quaternion attitude calculation by using the acceleration and the angular velocity, and calculating the inclination angle of the equipment in the transverse left-right direction.
And step 203, acquiring the current rotating speed of the flywheel.
In this embodiment, the rotation speed acquisition circuit is used to measure the current rotation speed of the flywheel, and the current rotation speeds of the flywheel and the motor are the same because the flywheel and the motor are coaxial. According to specific conditions, the rotating speed acquisition circuit can also acquire the current rotating speed of the motor. Wherein, the rotational speed acquisition circuit mainly comprises an encoder.
In this embodiment, the MCU actively reads the current rotation speed of the flywheel measured by the rotation speed acquisition circuit, and in other optional implementations of this embodiment, the rotation speed acquisition motor actively transmits the measured current rotation speed of the flywheel to the MCU.
And 204, carrying out differential D control on the angular speed, carrying out proportional P control on the inclination angle, carrying out proportional integral PI control on the current rotating speed of the flywheel, and modulating a Pulse Width Modulation (PWM) signal by using the sum of the differential D control on the angular speed, the proportional P control on the inclination angle and the proportional integral PI control on the rotating speed of the flywheel.
In this embodiment, the acquired current rotation speed is subjected to first-order low-pass filtering, and the filtered data is converted into how many revolutions per second.
In the present embodiment, the duty ratio of the pulse width modulation PWM signal is modulated using a PID control algorithm. Referring to FIG. 3, which is a flow chart of the PID control algorithm, an angle set point is set as shown. The angle set value is the angle of left and right inclination under the condition that the longitudinal two-wheeled equipment is naturally balanced. And subtracting the angle set value from the inclination angle of the current actual left and right inclination of the equipment calculated in the previous step to obtain the angle deviation, and multiplying the angle deviation by the first proportional gain to obtain the proportion P control of the inclination angle. The inclination condition of the equipment can be quickly adjusted and improved through the proportional P control of the inclination angle, and the control equipment continuously returns to a natural equilibrium state from the inclination state under the condition of balancing the control equipment.
Continuing to refer to fig. 3, the angular velocity of the device is subjected to differential D control, specifically, the angular velocity of the gyroscope on the left and right transverse axes is differentiated, that is, the angular acceleration of the gyroscope is calculated, whether the device has a tendency of accelerating inclination is judged according to the angular acceleration, before a large inclination angle is generated, the inclination of the device is prevented and controlled in advance through angular velocity differential control, the timeliness is better, if the angular velocity is reduced, the angular acceleration is a negative value, the differential D control outputs a negative value, a reverse action is generated, and the balance is adjusted. In addition, reasonable differential gain and differential time are set.
In this embodiment, the principle of the device balance is to control the flywheel to rotate at an angular acceleration to generate a torque to counter the torque generated by gravity when the device is inclined, so if the rotation speed of the flywheel is reduced by improper regulation, the flywheel will rotate faster and faster, the control is volatile, and the power is consumed, so the application performs proportional-integral control on the rotation speed of the flywheel. Referring to fig. 3, the value of the rotational speed set point is set, in this embodiment to zero, and in other implementations, the rotational speed set point is set to 2 revolutions per second or 3 revolutions per second, etc. And subtracting the set rotating speed value from the acquired current rotating speed of the flywheel to obtain a rotating speed deviation, and multiplying the rotating speed deviation by a second proportional gain to obtain the proportional control of the rotating speed of the flywheel. The integral of the rotational speed deviation is calculated for integral control, usually an integration time is set, the sum of all rotational speed deviations within the integration time is calculated, and then the sum is multiplied by an integral gain to obtain the integral I control of the rotational speed of the flywheel. In order to prevent the integral overshoot and oscillation and make the whole PID control more stable and accurate, in this embodiment, the amplitude of the integral I control is limited, i.e. less than or equal to a certain preset amplitude threshold.
Continuing with fig. 3, the differential D control of the angular velocity, the proportional P control of the tilt angle, the proportional P control of the flywheel speed, and the integral I control are summed to limit the magnitude of the sum for optimization, and the duty cycle of the PWM signal is then modulated with the limited sum.
And step 205, driving the motor to drive the flywheel to rotate by adopting the PWM signal, generating torque and maintaining the balance of the equipment.
With continued reference to fig. 4, which is a schematic illustration of the present embodiment, as shown, O represents the center of gravity of the device, the dashed line represents the ground, and D represents the point of contact of the device with the ground, which point of contact does not refer to the actual front or rear wheel contact point, but rather to a projection of the center of gravity of the device along the ground of the device. For example, if the device is front-to-back symmetric and the device is placed vertically on the ground, the contact point is the projection of the center of gravity on the ground, and if the device is tilted left and right in place, the position of the contact point is unchanged and the distance of the OD is L. As shown, θ is the angle between the center of gravity of the device and the line connecting the device with the ground contact point and the ground. OA represents the deviceThe force of gravity, AB, represents a component of gravity that is offset by the friction of the device with the ground. OB represents the component of gravity in the direction perpendicular to the device, as the presence of this component results in the device continually tilting left or right. Moment T generated by the component force1Comprises the following steps:
T1＝M×g×cosθ×L
where M is the mass of the device and g is the acceleration of gravity.
Controlling the flywheel to rotate to generate a torque T1Equal and opposite moments for counteracting the moment T1And keeping the equipment balanced.
The moment of inertia J of the flywheel is:
where m is the mass of the flywheel, R is the outer radius of the flywheel, and R is the inner radius of the flywheel.
The moment generated by the rotation of the flywheel is equal to the product of the moment of inertia and the angular acceleration, so the moment T of the flywheel2Comprises the following steps:
when T is1＝T2Angular acceleration α of flywheel:
in the implementation, the parameters of the PID are regulated and controlled, so that the duty ratio of the PWM signal is modulated, the angular acceleration of the flywheel is controlled, the equipment generates reverse action torque, and the balance of the equipment is controlled. Wherein, the PID parameters include: first proportional gain, second proportional gain, derivative time, integral gain, integral time.
During actual implementation, the PID parameters are adjusted according to the actual balance condition of the equipment, the PID algorithm is optimized, and the equipment is controlled to keep balance.
In this embodiment, the angular velocity and the acceleration of the device are obtained first, the attitude fusion is performed, the tilt angle of the device is calculated, and then the current rotation speed of the flywheel is obtained. The method comprises the steps of carrying out differential D control on the angular speed of equipment, preventing and treating the inclination of the equipment in advance, carrying out proportional P control on the inclination angle of the equipment, quickly adjusting and improving the inclination condition of the equipment, carrying out proportional integral PI control on the current rotating speed of a flywheel, quickly reducing the rotating speed of the flywheel and eliminating the rotating speed accumulated by inertial rotation of the flywheel. Under the condition of maintaining the balance of the equipment, controlling the inclination angle of the equipment to return to a set angle value and return to a natural balance state; and controlling the rotation speed of the flywheel to be reduced to a rotation speed set value, and returning to the state that the flywheel stops rotating and is static.
In other optional implementation manners of the embodiment, the flywheel motor is provided with a bluetooth chip, which supports bluetooth communication with the upper computer, and supports readjustment of the PID parameters by the upper computer when the flywheel motor is installed on a different device. Wherein, the host computer includes but not limited to: smart phone, panel computer, remote control handle.
In other optional implementation manners of this embodiment, the device is a remote control motorcycle, which includes a power motor module and a steering engine module in addition to a flywheel motor, wherein the power motor module drives a rear wheel to rotate to control the forward or backward speed of the motorcycle. The steering engine module drives the front wheel to steer so as to control the driving direction of the motorcycle. The flywheel motor, the power motor module, the steering engine module and the upper computer are all provided with Bluetooth mesh chips and used as a node to support and form a Bluetooth mesh network. The nodes in the bluetooth mesh network may communicate with each other. And a user is supported to send control instructions to the flywheel motor, the power motor module and the steering engine module through an upper computer, and in addition, parameter information sent by the flywheel motor, the power motor module and the steering engine module is received. For example, the smart phone sends a motion command to the power motor module through the bluetooth mesh network, wherein the command includes a rotation direction, a rotation speed and a rotation angle of the motor. The power motor module receives the motion instruction, analyzes the instruction corresponding to the power motor module, and controls the motor to rotate according to the instruction so as to enable the remote control motorcycle to move forwards or backwards. And then, the power motor module completely forwards the received command to the steering engine module in a mesh adv mode, and the steering engine module analyzes the command corresponding to the power motor module from the command to complete the action of rotating angle so as to enable the remote control motorcycle to turn left or right. In the process, the remote control motorcycle executes the motions of advancing, retreating, turning left and turning right according to the instructions, and the balance of the remote control motorcycle is maintained through the flywheel motor in the process.
The method for controlling the balance of the equipment can be applied to the longitudinal two-wheel equipment and the transverse two-wheel equipment. The tilt angle of the front and back tilt of the device is calculated according to the angular velocity and the acceleration. The angle set value is subtracted from the inclination angle to obtain an angle deviation, and then the angle deviation is multiplied by a proportional gain to obtain a proportional P control of the inclination angle. Wherein the set angle value is set to zero. And D, carrying out differential control on the angular speed of the device in the front and rear axial directions, and carrying out proportional integral PI control on the rotating speed of the flywheel. I.e. the balancing of the device is controlled by a PID algorithm.
With continuing reference to fig. 5, a schematic structural diagram of an apparatus embodiment corresponding to the method embodiment described above is shown. As shown, the device includes an obtaining
In this embodiment, the PID control unit includes: the device comprises an angular speed control subunit, an angle control subunit, a flywheel rotating speed control subunit and a modulation subunit. The angular velocity control subunit is configured to perform differential calculation on the angular velocity of the device, and then multiply the angular velocity by a differential gain to obtain a differential D control of the angular velocity; the angle control subunit is used for setting an angle set value of the equipment, subtracting the angle set value from the inclination angle calculated in the attitude calculation unit to obtain an angle deviation, and multiplying the first proportional gain by the angle deviation to obtain a proportional P control of the inclination angle; the flywheel rotation speed control subunit is configured for setting a rotation speed set value of the flywheel, subtracting the rotation speed set value from the current rotation speed of the flywheel to obtain a rotation speed deviation, multiplying the rotation speed deviation by a second proportional gain to obtain a proportional P control of the rotation speed of the flywheel, integrating the rotation speed deviation, and multiplying the integral P control by an integral gain to obtain an integral I control of the rotation speed of the flywheel; and a modulation subunit configured to calculate the sum of the derivative of angular velocity, the proportional of tilt angle, the proportional of flywheel rotation, the integral, I, and modulate the duty cycle of the pulse width modulated, PWM, signal.
In other embodiments, the apparatus further includes a bluetooth communication unit configured to form a bluetooth mesh network with other modules of the device and/or the host computer to perform bluetooth communication. And receiving and storing the adjusted PID parameters sent by the upper computer through a Bluetooth mesh network.
In other embodiments, the apparatus further comprises an angular acceleration calculation unit and a PID parameter adjustment unit, wherein the angular acceleration calculation unit is configured to calculate the angular acceleration of the flywheel when the moment of the device is equal to the moment of the flywheel; and the PID parameter adjusting unit is configured for adjusting PID parameters according to the angular acceleration of the flywheel and the balance condition of the equipment. So as to be suitable for different equipment to keep the balance of the equipment.
The above description is only a preferred embodiment of the application and is illustrative of the principles of the technology employed. It will be appreciated by a person skilled in the art that the scope of the invention as referred to in the present application is not limited to the embodiments with a specific combination of the above-mentioned features, but also covers other embodiments with any combination of the above-mentioned features or their equivalents without departing from the inventive concept. For example, the above features may be replaced with (but not limited to) features having similar functions disclosed in the present application.
- 上一篇：一种医用注射器针头装配设备
- 下一篇：负载转动惯量辨识方法及系统