CN112578712B - Two-wheeled robot motion and control method - Google Patents
Two-wheeled robot motion and control method Download PDFInfo
- Publication number
- CN112578712B CN112578712B CN202011472235.2A CN202011472235A CN112578712B CN 112578712 B CN112578712 B CN 112578712B CN 202011472235 A CN202011472235 A CN 202011472235A CN 112578712 B CN112578712 B CN 112578712B
- Authority
- CN
- China
- Prior art keywords
- control
- motion
- singlechip
- data
- communicated
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
- G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
- G05B19/0423—Input/output
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/20—Pc systems
- G05B2219/24—Pc safety
- G05B2219/24215—Scada supervisory control and data acquisition
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
The invention provides a two-wheeled robot motion and control method, which comprises two parts, namely motion control and environment monitoring. The motion control adopts a gyroscope acceleration sensor MPU6050 to enable an internal DMP to complete data fusion of a gyroscope and an accelerometer, so that the pitching state and the state change rate of the robot are detected, PWM is calculated through a high-speed central processing unit, a TB6612FNG chip driving motor is controlled to generate corresponding acceleration, so that the gesture control of the robot is realized, environmental monitoring adopts a hygrothermograph DHT11, a barometer MS5611, an illuminometer BH1750, an air quality sensor MQ135 and a fire source detector to monitor environmental parameters, and environmental information and alarm information can be acquired in real time through a loudspeaker, an OLED display and an upper computer. The invention has the beneficial effects of realizing seven motion control of front, back, left, right, rotation, speed regulation and stopping, ultrasonic ranging and obstacle avoidance, and actively stopping motion when encountering obstacles, and monitoring environmental parameters such as temperature and humidity, illumination, air pressure, altitude, air quality and the like.
Description
Technical Field
The invention belongs to the field of machinery, and particularly relates to a two-wheeled robot motion and control method.
Background
With the current high-speed development of economy and popularization of mobile terminals, people put higher demands on environment, life quality and even travel modes, so that a new system must be developed, people can be helped to monitor environment information, wireless check and even control on the mobile terminals are required, and even a novel carrier with high cost performance and certain advantages is required to be adopted. At present, the control system and the control theory are more and more universal, so that the two-wheel balance system is more and more complete, and the system has the characteristics of portability, flexibility, small volume, high cost performance and the like, is quite suitable as a carrier of a monitoring system, and can be used for monitoring and entertainment by a user.
In addition, people put higher requirements on environment and life quality, the environment and tasks faced by the mobile robot are more and more complex, and the robot needs to work in some working places with narrow space and more corners frequently. How to flexibly and quickly perform various tasks in a complex environment is a hot spot of current research.
Disclosure of Invention
The invention aims to provide a two-wheeled robot with flexible movement and multiple functions, which is particularly suitable for monitoring work in narrow dangerous occasions.
The technical scheme of the invention is as follows: the motion control part adopts a 3S lithium battery and a direct current speed reducing motor with an encoder, the controller adopts STM32-Cortex-M3, the attitude sensor adopts a 6-axis attitude sensor MPU6050, the driving circuit adopts TB6612FNG, the environment monitoring adopts a STM32-Cortex-M3 controller, the hygrometer adopts DHT11, the illuminometer adopts BH1750, and the air quality sensor adopts MQ135;
the restoring force required for the two-wheeled robot to maintain balance is:
F＝mgsinθ-macosθ≈mgθ-mk 1 θ
in the equation, since θ is small, linearization is performed. In order to keep the stable increase of the damping force, the damping force is proportional to the speed of the deflection angle and the direction is opposite. Thus, formula 2.2-1 may become:
F＝mgθ-mk 1 θ-mk 2 θ ′
thereby obtaining the acceleration control algorithm of the two-wheeled robot: a=k 1 θ+k 2 θ, where θ: inclination angle of the vehicle model, theta': angular velocity, k 1 、k 2 : a scaling factor;
vertical ring control: the control algorithm of the acceleration of the two-wheeled robot is abstracted into a PD controller, the angle theta is read from the MPU6050-DMP through an IIC communication protocol, the angular velocity omega is read from the MPU 6050-gyroscope, the proportional coefficient P and the differential coefficient D are respectively multiplied, and the added angular velocity omega is output to PWM control the motor speed;
speed loop control: the speed control adopts a PI control algorithm, first-order low-pass filtering is carried out on data, and the algorithm formula of the first-order low-pass filtering is as follows: y (n) = oc X (n) (1-oc) Y (n-1), wherein oc is a filter coefficient, Y (n) is the current sampling value, Y (n-1) is the last time filter output value, Y (n) is the current time filter output value, PI control is carried out after a first-order low-pass filter algorithm, in P control, a value after the first-order low-pass filter is multiplied by a proportional coefficient P, and in I control, integral accumulation and integral limiting are carried out on data and then integral coefficient I is multiplied;
steering and PI conversion control: in the vertical and straight running states, the proportional control is closed, the simple differential D control is adopted, in the steering state, the differential control is closed, the simple proportional P control is adopted, the inverse proportional operation is carried out according to the speed of the current system, the coefficient Cons is multiplied, the speed stepping value is obtained, and the proportional coefficient P is multiplied after accumulation;
vertical walking control: fusing the vertical ring PD control, the speed change PI control and the steering ring PD control, calculating the final PWM, and outputting to a motor driving chip TB6612FNG;
the hardware part comprises a main control part and a voice output part, wherein the main control part comprises a motion controller and an environment detection controller, the motion controller and the environment detection controller are communicated through a serial port USART, an external 8MHz crystal oscillator is obtained through configuration of a phase-locked loop frequency multiplication of 72MHz, a reset circuit is input through a 10k resistor pull-up, a power supply circuit adopts a voltage stabilizing chip AMS1117-3.3 to stabilize voltage to 3.3V, an MPU6050 is communicated with a singlechip through an IIC, IIC pins are PB8 and PB9, ultrasonic ranging is connected with the singlechips PB0 and PB1, acquisition time of a capturing mode is input through a timer and converted into a distance through sound velocity, PWM and rotation directions are obtained from the singlechips through driving of the motors, the motors are output to the motors, a barometer MS5611 and an illuminometer BH1750 are communicated with the singlechips through the IIC, the DHT11 is communicated with the singlechips through a single bus, and the singlechips acquire air quality and an input mode through an ADC;
the voice output part comprises a voice module, the voice module is communicated with the singlechip through the serial UART, and the singlechip controls the playing to stop and the track number by sending corresponding instructions.
Further, the main control part is provided with Bluetooth, the Bluetooth is communicated with the singlechip through a serial port USART, and the singlechip adopts PB10 and PB11.
Further, the lighting circuit of the main control part utilizes the photosensitive characteristic of the photosensitive resistor and the amplifying function of the three-stage tube to enable the LED to adjust the brightness according to the brightness of the environment.
Further, the vertical walking realization method comprises the following steps: with 5ms external interrupt of the MPU6050, the motion gesture is read to the DMP and gyroscope via IIC communication protocol once every 5ms, including Y-axis DMP data, Y-axis gyroscope data, and Z-axis gyroscope data, encoder data, key scanning, and ultrasonic ranging are read, and PWM is calculated by the PID controller and output to the motor drive TB6612FNG.
Furthermore, the STM32 reads the motion gesture of the system through an IIC protocol, the DMP finishes data fusion and data filtering, directly reads Euler angles, and reads the angular velocities of a Y axis and a Z axis through a gyroscope.
Further, the MS5611 barometer is communicated with the singlechip through an IIC communication protocol, and the altitude of the position is obtained through comparison with the standard atmospheric pressure; the DHT11 is communicated with the singlechip in a single bus mode, reads five bytes of data altogether, and outputs a temperature and humidity value after the data are checked; the MQ-135 air quality sensor is communicated with the singlechip in an ADC mode, is sensitive to common air quality indexes, and defines six air quality grades: a+, A, B, C, D, E, comparing with the value acquired by ADC to obtain the current air quality grade; BH1750 communicates with the singlechip through IIC communication protocol, and the singlechip directly reads out the illuminance value. The fire detector adopts direct numerical output, and when flame is monitored, the level is turned over.
Further, the environmental monitoring singlechip is used for collecting environmental parameters: the temperature, humidity, illuminance, atmospheric pressure, altitude, air quality level and battery power are sent to a motion control singlechip, and the motion control singlechip superimposes motion attitude parameters, ultrasonic ranging, an encoder, PWM, a system mode and a system state, and sends the data in a format agreed by an upper computer. The bluetooth receives the environmental parameters, the motion parameters, and the GPS data, and wirelessly transmits them.
Parameter reception frame format:
byte0: frame start byte1 (A5)
Byte1: frame start byte2 (5A)
Byte2: the frame contains the number of bytes (data and check)
Byte3: frame function indication
Byte4: high eight bits of data
Byte5: eighth bit data
Byte6: data and data other than the start byte and end byte
Byte7：AA
The invention has the advantages and positive effects that:
1. the motion swing of the two-wheel vehicle is simulated, the required balance force can be achieved, and the functions of advancing, retreating, steering and accelerating and decelerating are completed through the cooperation of the vertical ring, the speed ring and the steering ring, so that the whole vehicle body is kept stable.
2. The two-wheeled robot can be used for monitoring dangerous operations instead of manpower by automatically controlling the remote control, the communication principle, the PWM direct current motor control technology, the radio frequency identification technology and the sensor technology.
3. Through transmission and display device, can remote monitoring and detection environment, can detect the work to the manual work is difficult to reach or dangerous work area, greatly reduced personnel injury and danger coefficient.
4. Through the chip realization self-balancing, the two-wheeled will be nimble more convenient, possess better steering capacity, have better light and steering capacity than the four-wheeled vehicle.
Drawings
FIG. 1 is a system architecture diagram of the present invention;
FIG. 2 is a flow chart of the motion control singlechip operation of the invention;
FIG. 3 is an operational flow of the environmental monitoring SCM of the present invention;
FIG. 4 is a graph of an upstanding ring control algorithm of the invention;
FIG. 5 is a diagram of a speed loop control algorithm of the present invention;
FIG. 6 is a diagram of a steering ring control algorithm of the present invention;
FIG. 7 is a graph of the upright walking control algorithm of the present invention;
FIG. 8 is a diagram of an upright walking implementation of the present invention;
FIG. 9 is a diagram of an environmental monitoring implementation of the present invention;
fig. 10 is a diagram of a wireless communication implementation of the present invention;
fig. 11 is a hardware schematic of the present invention.
Detailed Description
The present invention will be described in detail with reference to the accompanying drawings.
As shown in fig. 1-11:
the motion control part adopts a 3S lithium battery and a direct current speed reducing motor with an encoder, the controller adopts STM32-Cortex-M3, the attitude sensor adopts a 6-axis attitude sensor MPU6050, the driving circuit adopts TB6612FNG, the environment monitoring adopts a STM32-Cortex-M3 controller, the hygrometer adopts DHT11, the illuminometer adopts BH1750, and the air quality sensor adopts MQ135;
the restoring force required for the two-wheeled robot to maintain balance is:
F＝mgsinθ-macosθ≈mgθ-mk 1 θ
in the equation, since θ is small, linearization is performed. In order to keep the stable increase of the damping force, the damping force is proportional to the speed of the deflection angle and the direction is opposite. Thus, formula 2.2-1 may become:
F＝mgθ-mk 1 θ-mk 2 θ′
thereby obtaining the acceleration control algorithm of the two-wheeled robot: a=k 1 θ+k 2 θ, where θ: inclination angle of the vehicle model, theta': angular velocity, k 1 、k 2 : a scaling factor;
vertical ring control: the control algorithm of the acceleration of the two-wheeled robot is abstracted into a PD controller, the angle theta is read from the MPU6050-DMP through an IIC communication protocol, the angular velocity omega is read from the MPU 6050-gyroscope, the proportional coefficient P and the differential coefficient D are respectively multiplied, and the added angular velocity omega is output to PWM control the motor speed;
speed loop control: the speed control adopts a PI control algorithm, first-order low-pass filtering is carried out on data, and the algorithm formula of the first-order low-pass filtering is as follows: y (n) = oc X (n) (1-oc) Y (n-1), wherein oc is a filter coefficient, Y (n) is the current sampling value, Y (n-1) is the last time filter output value, Y (n) is the current time filter output value, PI control is carried out after a first-order low-pass filter algorithm, in P control, a value after the first-order low-pass filter is multiplied by a proportional coefficient P, and in I control, integral accumulation and integral limiting are carried out on data and then integral coefficient I is multiplied;
steering and PI conversion control: in the vertical and straight running states, the proportional control is closed, the simple differential D control is adopted, in the steering state, the differential control is closed, the simple proportional P control is adopted, the inverse proportional operation is carried out according to the speed of the current system, the coefficient Cons is multiplied, the speed stepping value is obtained, and the proportional coefficient P is multiplied after accumulation;
vertical walking control: fusing the vertical ring PD control, the speed change PI control and the steering ring PD control, calculating the final PWM, and outputting to a motor driving chip TB6612FNG;
the hardware part comprises a main control part and a voice output part, wherein the main control part comprises a motion controller and an environment detection controller, the motion controller and the environment detection controller are communicated through a serial port USART, an external 8MHz crystal oscillator is obtained through configuration of a phase-locked loop frequency multiplication of 72MHz, a reset circuit is input through a 10k resistor pull-up, a power supply circuit adopts a voltage stabilizing chip AMS1117-3.3 to stabilize voltage to 3.3V, an MPU6050 is communicated with a singlechip through an IIC, IIC pins are PB8 and PB9, ultrasonic ranging is connected with the singlechips PB0 and PB1, acquisition time of a capturing mode is input through a timer and converted into a distance through sound velocity, PWM and rotation directions are obtained from the singlechips through driving of the motors, the motors are output to the motors, a barometer MS5611 and an illuminometer BH1750 are communicated with the singlechips through the IIC, the DHT11 is communicated with the singlechips through a single bus, and the singlechips acquire air quality and an input mode through an ADC;
the voice output part comprises a voice module, the voice module is communicated with the singlechip through the serial UART, and the singlechip controls the playing to stop and the track number by sending corresponding instructions.
The main control part is provided with Bluetooth, the Bluetooth is communicated with a singlechip through a serial port USART, and the singlechip adopts PB10 and PB11.
The lighting circuit of the main control part utilizes the photosensitive characteristic of the photosensitive resistor and the amplifying function of the three-stage tube to enable the LED to adjust the brightness according to the brightness of the environment.
The vertical walking realization method comprises the following steps: with 5ms external interrupt of the MPU6050, the motion gesture is read to the DMP and gyroscope via IIC communication protocol once every 5ms, including Y-axis DMP data, Y-axis gyroscope data, and Z-axis gyroscope data, encoder data, key scanning, and ultrasonic ranging are read, and PWM is calculated by the PID controller and output to the motor drive TB6612FNG.
STM32 reads the motion gesture of the system through IIC protocol, the DMP completes data fusion and data filtering, the Euler angle is directly read, and the gyroscope reads the angular speeds of the Y axis and the Z axis.
The MS5611 barometer is communicated with the singlechip through an IIC communication protocol, and the altitude of the position is obtained by comparing the barometer with the standard atmospheric pressure; the DHT11 is communicated with the singlechip in a single bus mode, reads five bytes of data altogether, and outputs a temperature and humidity value after the data are checked; the MQ-135 air quality sensor is communicated with the singlechip in an ADC mode, is sensitive to common air quality indexes, and defines six air quality grades: a+, A, B, C, D, E, comparing with the value acquired by ADC to obtain the current air quality grade; BH1750 communicates with the singlechip through IIC communication protocol, and the singlechip directly reads out the illuminance value. The fire detector adopts direct numerical output, and when flame is monitored, the level is turned over.
The working procedure of this example:
actual test debugging:
setting parameters of an upright ring:
the system stand-up loop uses a proportional-derivative controller in a PID control algorithm, which is necessary because the system responds quickly to disturbances.
Proportional differential control code:
int balance(float Angle,float Gyro)
{
float Bias,kp＝350,kd＝1.2；int balance；
Bias＝Angle-0；
calculating vertical deviation
balance＝kp*Bias+Gyro*kd；
Vertical PWM/calculation
return balance；
Return to vertical PWM
}
The standing ring function uses the system tilt angle from the DMP and the gyroscope from the gyroscope for closed loop control, multiplied by the proportional coefficient K and the derivative coefficient D, respectively, to calculate PWM.
When setting the standing ring parameters, other PID control needs to be turned off to prevent interference.
Balance_Pwm＝balance(Angle_Balance,Gyro_Balance)；
//Velocity_Pwm＝velocity(Encoder_Left，Encoder_Right)；
//Turn_Pwm＝turn(Encoder_Left,Encoder_Right,Gyro_Turn)；
And (3) mechanical median setting of the system:
after the system is started, the system is slowly rotated back and forth, the angle value of the critical balance of the system is recorded, the angle value is read by an OLED, the angle value is the mechanical median value of the system, and the mechanical median value of the system is in a neutral state, namely the median value is zero.
Determining a proportionality coefficient P:
first, the range of the scale factor is estimated. When PWM is set to 7200 and the duty cycle is 100%, the system should be at about ±20° full rotation, i.e. the scaling factor is around 360, depending on the actual situation and experience. The upstanding loop of the system is a negative feedback system so the scaling factor is positive. From the test results, when the scaling factor is negative, the motor will accelerate to tilt the system down, which is clearly a positive feedback system.
In the rule of automatic control principle PID parameter setting, the proportional coefficient needs to be gradually increased, and when the system continuously shakes, the proportional coefficient is an ideal value.
Setting the scaling factor to 360, the system tends to remain balanced, but the corresponding speed is still not fast enough.
Setting the scaling factor to 500, the system tends to stay balanced faster, and while there is low frequency jitter, it is still somewhat inadequate.
Setting the proportionality coefficient to be 580, the tendency of the system to keep balance is faster, the jitter amplitude is large, and the proportionality coefficient is proved to meet the requirement.
Determining a differential coefficient D:
the maximum angular velocity of the gyroscope output is typically less than 5 digits, and the differential coefficient should be no greater than 2 in combination with practical experience and PID control principles.
Determining a proportionality coefficient to be 580;
in the rule of automatic control principle PID parameter setting, the principle of determining differential coefficient is: the differential coefficient increases until high frequency jitter occurs.
The differential coefficient is set to be 1, the system shake is greatly improved, and the motor is correspondingly accelerated.
Setting the differential coefficient to 2, the system jitters fast and strongly, indicating that the differential coefficient is already large enough.
The maximum value of the proportional coefficient is 580 and the maximum value of the differential coefficient is 2. The coefficient of the law of PID parameter tuning according to the automatic control principle is usually obtained by multiplying the maximum value by a coefficient of 0.6 and rounding to obtain p=350 and d=1.2. However, due to the lack of a braking mechanism, when the PWM duty cycle is large, a sufficiently large acceleration cannot be provided, resulting in a system that cannot stay upright for a long period of time.
Setting a speed loop PI parameter:
in the speed controller, proportional-integral control is a classical control algorithm, and integration is helpful for reducing the residual difference between the target speed and the target speed, and has great significance in keeping the system upright and regulating speed.
Proportional-integral control code:
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder；
static float Encoder_Integral；
float kp＝80,ki＝0.4；
Encoder_Least＝(Encoder_Left+Encoder_Right)-0；
Encoder*＝0.7；
Encoder+＝Encoder_Least*0.3；
Encoder_Integral+＝Encoder；
Velocity＝Encoder*kp+Encoder_Integral*ki；return Velocity；
}
the controller first calculates the speed deviation, first performs first order low pass filtering, then integrates and accumulates to obtain PWM in order to reduce interference to the vertical ring and enhance stability.
The standing ring, which has been commissioned before opening:
Balance_Pwm-balance(Angle_Balance,Gyro_Balance)；
Velocity_Pwm＝velocity(Encoder_Left，Encoder_Right)；
//Turn_Pwm＝turn(Encoder_Left,Encoder_Right,Gyro_Turn)：
the integral term is obtained by integrating deviation, so that the polarity of integral control and proportional control are the same, in the rule of PID parameter setting of an automatic control principle, PID parameters have a certain proportional relation with each other, and tests show that: setting the integral coefficient to P/200 has a desirable effect, so only the scaling factor needs to be determined.
The system sets that when the speed deviation reaches the maximum speed of the motor, the maximum PWM is output, so the estimated proportionality coefficient is:
the stand-up loop is opened for joint debugging until the system can be kept balanced and the speed is almost the same as the target value.
The system can be kept substantially upright, but with a large swing, with a scaling factor of 60 and an integration factor of 0.3.
Setting the proportional coefficient to 80 and the integral coefficient to 0.4, the system can be balanced and the speed is almost the same as the target value.
Setting the proportional coefficient as 100, the integral coefficient as 0.5, and the sensitivity of the system is too high, and the weak interference can lead the system to swing back and forth.
Therefore, a proportionality coefficient of 80 and an integral coefficient of 0.4 is an ideal value for the speed loop.
Setting PD parameters of a steering ring:
the steering ring has low response requirement, so that the proportional control is used only, namely the data of the Z-axis gyroscope is used as steering speed deviation to carry out P control, the steering speed is kept to be a set value, and the method has the advantages of being simple in algorithm, avoiding the phenomenon that the encoder cannot monitor the wheel sliding, the gyroscope drifting and the like. The goal is to match the vertical ring and the speed ring, so that the track of the system running in a short distance is close to a straight line.
Determining a proportionality coefficient:
the previously commissioned standing ring and speed ring are closed first:
//Balance_Pwm＝balance(Angle_Balance,Gyro_Balance)；
//Velocity_Pwm＝velocity(Encoder_Left,Encoder_Right)；
Turn Pwm＝turn(Encoder Left,Encoder Right,Gyro)
setting the proportionality coefficient to be-1, the system can be easily rotated to indicate polarity errors when the system is pressed to rotate on the ground. Setting the scaling factor to 1, tests have shown that it is difficult to turn the system with great force, so the polarity is positive.
Setting the scaling factor, opening the standing ring and the speed ring at this time:
Balance_Pwm＝balance(Angle_Balance,Gyro_Balance)；
Velocity_Pwm＝velocity(Encoder_Left,Encoder_Right)；
Turn_Pwm＝turn(Encoder_Left,Encoder_Right,Gyro_Turn)；
the proportional coefficient is set to be 0.2, steering control is weak, and deviation of the running line is very large.
Setting the proportionality coefficient to 0.6 increases the response of the angular velocity control, but still has a large deviation from the straight line.
The effect of the system straight line is ideal by setting the proportionality coefficient to be 1.
The proportional coefficient is set to be 1.6, and the system has better linear effect and can shake severely when stopping suddenly.
Therefore, it can be determined that p=1 is an ideal value of the steering control P parameter.
Double-controller communication debugging:
the MCU2 mainly collects environmental data such as temperature and humidity, illuminance, air quality, battery voltage, atmospheric pressure, etc., and then transmits the data to the MCU1 through USART, and after the data processing, the data are displayed on the main display screen OLED1, and transmitted to the upper computer through bluetooth together with system operation parameters such as pitch angle, PWM data, status flags, etc. By monitoring the first two bits of data, the data type is identified, and the data in the data buffer is stored in different arrays and displayed in different areas of the OLED.
The data of the data buffer USART RX BUF is put into the array DH Temp [ ] bit by comparing the first two bits of data.
The foregoing describes one embodiment of the present invention in detail, but the description is only a preferred embodiment of the present invention and should not be construed as limiting the scope of the invention. All equivalent changes and modifications within the scope of the present invention are intended to be covered by the present invention.
Claims (6)
1. A two-wheeled robot motion and control method is characterized in that: the intelligent control system comprises motion control and environment monitoring, wherein a 3S lithium battery and a direct current gear motor with an encoder are adopted as the motion control part, an STM32-Cortex-M3 is adopted as the controller, a 6-axis attitude sensor MPU6050 is adopted as the attitude sensor, a TB6612FNG is adopted as the driving circuit, an STM32-Cortex-M3 controller is adopted as the environment monitoring, a DHT11 is adopted as the hygrometer, a BH1750 is adopted as the illuminometer, and an MQ135 is adopted as the air quality sensor;
the restoring force required for the two-wheeled robot to maintain balance is:
F＝mgsinθ-macosθ≈mgθ-mk 1 θ
in the formula, since θ is small, linearization is performed to keep stable and increase the damping force, the damping force is proportional to the speed of the deflection angle, and the direction is opposite, so the restoring force formula can be changed as follows:
F＝mgθ-mk 1 θ-mk 2 θ′
thereby obtaining the acceleration control algorithm of the two-wheeled robot: a=k 1 θ+k 2 θ, where θ: inclination angle of the vehicle model, theta': angular velocity, k 1 、k 2 : a scaling factor;
vertical ring control: the control algorithm of the acceleration of the two-wheeled robot is abstracted into a PD controller, the angle theta is read from the MPU6050-DMP through an IIC communication protocol, the angular velocity omega is read from the MPU 6050-gyroscope, the proportional coefficient P and the differential coefficient D are respectively multiplied, and the added angular velocity omega is output to PWM control the motor speed;
speed loop control: the speed control adopts a PI control algorithm, first-order low-pass filtering is carried out on data, and the algorithm formula of the first-order low-pass filtering is as follows: y (n) = oc X (n) (1-oc) Y (n-1), wherein oc is a filter coefficient, X (n) is the current sampling value, Y (n-1) is the last time filter output value, Y (n) is the current time filter output value, PI control is carried out after a first-order low-pass filter algorithm, in P control, a value after the first-order low-pass filter is multiplied by a proportional coefficient P, and in I control, integral accumulation and integral limiting are carried out on data and then integral coefficient I is multiplied;
steering and PI conversion control: in the vertical and straight running states, the proportional control is closed, the simple differential D control is adopted, in the steering state, the differential control is closed, the simple proportional P control is adopted, the inverse proportional operation is carried out according to the speed of the current system, the coefficient Cons is multiplied, the speed stepping value is obtained, and the proportional coefficient P is multiplied after accumulation;
vertical walking control: fusing the vertical ring PD control, the speed change PI control and the steering ring PD control, calculating the final PWM, and outputting to a motor driving chip TB6612FNG;
the hardware part comprises a main control part and a voice output part, wherein the main control part comprises a motion controller and an environment detection controller, the motion controller and the environment detection controller are communicated through a serial port USART, an external 8MHz crystal oscillator is obtained through configuration of a phase-locked loop frequency multiplication of 72MHz, a reset circuit is input through a 10k resistor pull-up, a power supply circuit adopts a voltage stabilizing chip AMS1117-3.3 to stabilize voltage to 3.3V, an MPU6050 is communicated with a singlechip through an IIC, IIC pins are PB8 and PB9, ultrasonic ranging is connected with the singlechips PB0 and PB1, acquisition time of a capturing mode is input through a timer and converted into a distance through sound velocity, PWM and a rotation direction are obtained from the singlechips through motor driving, the PWM and the rotation direction are output to a motor, a barometer MS5611 and an illuminometer BH1750 are communicated with the singlechips through the IIC, the DHT11 is communicated with the singlechips through a single bus, and the singlechips acquire air quality through an ADC;
the voice output part comprises a voice module, the voice module is communicated with the singlechip through the serial UART, and the singlechip controls the playing to stop and the track number by sending corresponding instructions.
2. The two-wheeled robot motion and control method of claim 1, wherein: the main control part is provided with Bluetooth, the Bluetooth is communicated with a singlechip through a serial port USART, and the singlechip adopts PB10 and PB11.
3. The two-wheeled robot motion and control method of claim 1, wherein: the lighting circuit of the main control part utilizes the photosensitive characteristic of the photosensitive resistor and the amplifying function of the three-stage tube to enable the LED to adjust the brightness according to the brightness of the environment.
4. The two-wheeled robot motion and control method of claim 1, wherein: the vertical walking realization method comprises the following steps: with 5ms external interrupt of the MPU6050, the motion gesture is read to the DMP and gyroscope via IIC communication protocol once every 5ms, including Y-axis DMP data, Y-axis gyroscope data, and Z-axis gyroscope data, encoder data, key scanning, and ultrasonic ranging are read, and PWM is calculated by the PID controller and output to the motor drive TB6612FNG.
5. The two-wheeled robot motion and control method of claim 1, wherein: STM32 reads the motion gesture of the system through IIC protocol, the DMP completes data fusion and data filtering, the Euler angle is directly read, and the gyroscope reads the angular speeds of the Y axis and the Z axis.
6. The two-wheeled robot motion and control method of claim 1, wherein: the MS5611 barometer is communicated with the singlechip through an IIC communication protocol, and the altitude of the position is obtained by comparing the barometer with the standard atmospheric pressure; the DHT11 is communicated with the singlechip in a single bus mode, reads five bytes of data altogether, and outputs a temperature and humidity value after the data are checked; the MQ-135 air quality sensor is communicated with the singlechip in an ADC mode, is sensitive to common air quality indexes, and defines six air quality grades: a+, A, B, C, D, E, comparing with the value acquired by ADC to obtain the current air quality grade; BH1750 is communicated with a singlechip through an IIC communication protocol, and the singlechip directly reads out an illuminance value, and a fire source detector adopts direct numerical output, so that when flame is monitored, the level is turned over.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011472235.2A CN112578712B (en) | 2020-12-15 | 2020-12-15 | Two-wheeled robot motion and control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011472235.2A CN112578712B (en) | 2020-12-15 | 2020-12-15 | Two-wheeled robot motion and control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112578712A CN112578712A (en) | 2021-03-30 |
| CN112578712B true CN112578712B (en) | 2023-10-13 |
Family
ID=75135137
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011472235.2A Active CN112578712B (en) | 2020-12-15 | 2020-12-15 | Two-wheeled robot motion and control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112578712B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2010130179A1 (en) * | 2009-05-15 | 2010-11-18 | 北京工业大学 | Flexible two-wheel self-balance robot system and motion control method thereof |
| CN103676650A (en) * | 2013-11-29 | 2014-03-26 | 上海交通大学 | Method for PID (proportion integration differentiation) optimization control with dead zone for two-wheeled self-balancing intelligent vehicle |
| KR20160106442A (en) * | 2015-03-02 | 2016-09-12 | 한남대학교 산학협력단 | Balance walking device using gyro sensor and acceleration |
| CN108501768A (en) * | 2018-03-29 | 2018-09-07 | 南京航空航天大学 | A kind of two-wheeled method for control speed based on Z axis gyroscope and difference in wheel |
| CN109240305A (en) * | 2018-10-19 | 2019-01-18 | 广州大学华软软件学院 | Coaxial two wheels robot kinetic control system and method based on complementary filter |
| CN209765335U (en) * | 2019-03-12 | 2019-12-10 | 黄山学院 | Two-wheeled self-balancing mobile robot control system |
| CN112051842A (en) * | 2020-07-29 | 2020-12-08 | 浙江工业大学 | Obstacle crossing motion control method of two-wheeled self-balancing mobile robot |
-
2020
- 2020-12-15 CN CN202011472235.2A patent/CN112578712B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2010130179A1 (en) * | 2009-05-15 | 2010-11-18 | 北京工业大学 | Flexible two-wheel self-balance robot system and motion control method thereof |
| CN103676650A (en) * | 2013-11-29 | 2014-03-26 | 上海交通大学 | Method for PID (proportion integration differentiation) optimization control with dead zone for two-wheeled self-balancing intelligent vehicle |
| KR20160106442A (en) * | 2015-03-02 | 2016-09-12 | 한남대학교 산학협력단 | Balance walking device using gyro sensor and acceleration |
| CN108501768A (en) * | 2018-03-29 | 2018-09-07 | 南京航空航天大学 | A kind of two-wheeled method for control speed based on Z axis gyroscope and difference in wheel |
| CN109240305A (en) * | 2018-10-19 | 2019-01-18 | 广州大学华软软件学院 | Coaxial two wheels robot kinetic control system and method based on complementary filter |
| CN209765335U (en) * | 2019-03-12 | 2019-12-10 | 黄山学院 | Two-wheeled self-balancing mobile robot control system |
| CN112051842A (en) * | 2020-07-29 | 2020-12-08 | 浙江工业大学 | Obstacle crossing motion control method of two-wheeled self-balancing mobile robot |
Non-Patent Citations (2)
| Title |
|---|
| 基于FPGA的FIR数字滤波器的设计与实现;杨国庆;;现代电子技术(第19期);191-193 * |
| 基于互补滤波的两轮机器人运动控制系统设计;夏国清;陈华珍;甄文劲;;自动化技术与应用(第10期);76-82 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112578712A (en) | 2021-03-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109240305B (en) | Two-wheeled robot motion control system and method based on complementary filtering | |
| CN107284551A (en) | A kind of sniffing robot for being applicable complicated landform | |
| CN101417711B (en) | Disturbance compensation mechanism of two axis balance annular shelf | |
| CN103955152B (en) | A kind of high precision electro ZISHU for labyrinth contest and using method | |
| CN105116897A (en) | Double-core high-speed four-wheeled picomouse full digital navigation servo controller | |
| CN110806754A (en) | Four-rotor aircraft attitude correction control system and method | |
| CN112578712B (en) | Two-wheeled robot motion and control method | |
| CN110109354B (en) | Self-adaptive sliding mode control method for counteractive wheel balance bicycle robot | |
| CN110162068A (en) | A kind of control method of self-balance robot | |
| CN100587644C (en) | Integrated single loop controller for camera optical axis stable tracing | |
| CN111273542B (en) | Cubic robot, control system and method, and design method | |
| CN116069019A (en) | Control system of multifunctional intelligent balance trolley and fuzzy self-adaptive PID method thereof | |
| CN105137978A (en) | Double-core high-speed four-wheel mini mouse controller | |
| Sarathy et al. | Implementation of efficient self balancing robot | |
| CN108909918A (en) | A kind of control circuit and its control method of balance car | |
| WO2021120739A1 (en) | Laser tracking self-balancing scooter control method | |
| CN210500286U (en) | Single-ball balance mobile robot | |
| CN108415448A (en) | A kind of winged control method and system of aerial crusing robot | |
| CN112298428A (en) | Control method of balance car, obstacle avoidance method of balance car and following operation method | |
| CN204674738U (en) | The electric-control system of electrodynamic balance car | |
| You et al. | Design of two-wheel balance car based on STM32 | |
| Liu et al. | Design of Multifunctional Cruise Balance Vehicle | |
| CN110941281A (en) | Laser tracking balance car control system | |
| Naik et al. | Self-balancing Robot Using Mono-Vision | |
| Lv et al. | Research and Design of Four Rotor UAV Based on Cascade PID |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |