CN110162068A - A kind of control method of self-balance robot - Google Patents

Abstract

The invention provides a control method for a self-balancing robot. A stable DC power drive module is provided through a power supply module, the control module sends an instruction to control the DC motor of the execution module to run according to the programming mode, and an encoder forms a feedback loop; The attitude detection module collects each attitude angle in the system and transmits it to the main controller to form a control loop to control the motor. The sensor in the attitude detection module can directly read the quaternion and acceleration, and accurately obtain the inclination value and angular velocity value, so that the stability of the self-balancing robot is higher, and the external environment can be adapted to the external environment. The interference of the self-balancing robot ensures its safety and stability. In addition, the algorithm pressure of the main controller is reduced, the accuracy of data transmission is improved, and the real-time performance of the system is improved.

Description

A control method for a self-balancing robot

technical field

The invention relates to the field of self-balancing robots, in particular to a control method of a self-balancing robot.

Background technique

Self-balancing robot is a new type of small vehicle. Its model is similar to a first-order inverted pendulum, which has the characteristics of nonlinearity and strong coupling. Its structural characteristics are that the two wheels are coaxial and the left and right are arranged in parallel. The self-balancing robot is a highly unstable system. The gyroscope has a good dynamic effect. However, due to the temperature drift of the gyroscope, the angle obtained after integration during static time will not A large deviation is generated; while smooth filtering and denoising are used for the accelerometer signal, a good static angle can be obtained, but it is easily disturbed by dynamic acceleration during the movement. Therefore, the use of accelerometer or gyroscope alone cannot obtain effective and reliable vehicle body attitude information, and this linear combination method makes it impossible to take into account the stability of the system. In the face of large interference, the system is unstable. Therefore, It is necessary to study a self-balancing robot with high stability to realize balanced walking, accurate walking and obstacle avoidance of the self-balancing robot.

A control method of a self-balancing robot, there are still many practical problems to be solved in its practical application, and no specific solutions have been proposed.

SUMMARY OF THE INVENTION

The purpose of the present invention is to propose a control method of a self-balancing robot to solve the problem.

In order to achieve the above object, the present invention adopts the following technical solutions:

A control method for a self-balancing robot includes an electrical system, wherein the electrical system is divided into a control module, an attitude detection module, a wireless communication module, an execution module, and a power supply module; wherein the power supply module provides a stable DC power drive module for other parts, The control module sends instructions to control the DC motor in the execution module to run according to the programming method, and the feedback loop is formed by the encoder; the change of the attitude angle in each direction when the system moves is measured by the attitude detection module, and the data is uploaded for analysis. At the same time, the data is fed back to the main controller to form a control loop;

The attitude detection module adopts the sensor to collect the angular velocity and acceleration signals, directly reads the quaternion and acceleration through the DMP of the sensor, and directly converts the quaternion into the robot inclination, and outputs the voltage and steering applied on the motor through the control module, Control the motor.

Optionally, the sensor is a six-axis inertial sensor.

Optionally, the six-axis inertial sensor includes a three-axis MEMS accelerometer and a three-axis MEMS gyroscope, and also includes a digital motion processor DMP, which can process the data collected by the three-axis MEMS accelerometer and the three-axis MEMS gyroscope. Fusion, complete attitude calculation independently.

Optionally, the six-axis inertial sensor can be connected with other digital sensors to expand into a nine-axis sensor, which can output a nine-axis signal to establish complete spatial attitude information.

Optionally, the three-axis MEMS gyroscope and the three-axis MEMS accelerometer collect the voltage values of the x-axis, the y-axis and the z-axis respectively, and then convert them into digital signals through the ADC, and finally transmit them to the host through the I2C bus. control chip.

Optionally, the formula for converting the quaternion into the inclination of the robot is:

Among them, the Pitch rotation angle is the required inclination angle of the self-balancing robot.

Optionally, data communication between the self-balancing robot and external equipment is implemented through a wireless communication module.

Optionally, the control module includes a core controller, and the core controller is a single-chip embedded computer system.

Optionally, an expression is set in the control module, angle: balance angle deviation; Gyro_y: y-axis angular velocity; V: velocity deviation; Vi: velocity deviation integral; Gyro_z: z-axis angular velocity, the expression is :

PWM=angle·Kp+Gyro_y·Kd+V·Kps+Vi·Kis+Gyro_z·Kpt.

Optionally, in the control module, a critical proportional method is used for parameter setting.

Compared with the prior art, the beneficial technical effects achieved by the present invention are:

1. The control method of the present invention adopts the sensor to collect acceleration and acceleration signals, and directly reads the quaternion and acceleration through the DMP that comes with the sensor. Realize the balanced walking, accurate walking and obstacle avoidance of the self-balancing robot.

2. The control method of the present invention can self-adapt to the external environment and at the same time can reduce the interference of the external environment on the self-balancing robot to the greatest extent, thereby ensuring its safety and stability.

3. The control method of the present invention can reduce the algorithm pressure of the main controller, improve the accuracy of data transmission, and improve the real-time performance of the system.

Description of drawings

The present invention can be further understood from the following description in conjunction with the accompanying drawings. The components in the figures are not necessarily to scale, emphasis instead being placed upon illustrating the principles of the embodiments. Like reference numerals designate corresponding parts throughout the different views.

1 is a general system diagram of a control method of a self-balancing robot in one of the embodiments of the present invention;

FIG. 2 is a flow chart of a control module of a control method for a self-balancing robot in one embodiment of the present invention.

FIG. 3 is a system diagram of parameter setting by the critical proportional method of a control method of a self-balancing robot in one embodiment of the present invention.

Detailed ways

In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be described in further detail below in conjunction with its embodiments; it should be understood that the specific embodiments described herein are only used to explain the present invention, not to limit the present invention. invention. Other systems, methods and/or features of the present embodiments will become apparent to those skilled in the art upon review of the following detailed description. It is intended that all such additional systems, methods, features and advantages be included within this description, be within the scope of the invention, and be protected by the accompanying claims. Additional features of the disclosed embodiments are described in the following detailed description and will be apparent from the following detailed description.

The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar components; in the description of the present invention, it should be understood that if there are terms “upper”, “lower”, “left”,

The orientation or positional relationship indicated by "right" and the like is based on the orientation or positional relationship shown in the accompanying drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the indicated device or component must have a specific orientation, It is constructed and operated in a specific orientation, so the terms describing the positional relationship in the accompanying drawings are only used for exemplary illustration, and should not be construed as a limitation on this patent. specific meaning.

The present invention is a control method of a self-balancing robot, and the following embodiments are described according to FIGS. 1-3:

Example 1:

A control method for a self-balancing robot includes an electrical system, wherein the electrical system is divided into a control module, an attitude detection module, a wireless communication module, an execution module, and a power supply module; wherein the power supply module provides a stable DC power drive module for other parts, The control module sends instructions to control the DC motor in the execution module to run according to the programming method, and the feedback loop is formed by the encoder; the change of the attitude angle in each direction when the system moves is measured by the attitude detection module, and the data is uploaded for analysis. At the same time, the data is fed back to the main controller to form a control loop;

The attitude detection module adopts the sensor to collect the angular velocity and acceleration signals, directly reads the quaternion and acceleration through the DMP of the sensor, and directly converts the quaternion into the robot inclination, and outputs the voltage and steering applied on the motor through the control module, Control the motor.

Wherein, the sensor is a six-axis inertial sensor; the six-axis inertial sensor includes a three-axis MEMS accelerometer and a three-axis MEMS gyroscope, and also includes a digital motion processor DMP, which can combine the three-axis MEMS accelerometer and the three-axis MEMS accelerometer with the three-axis MEMS gyroscope. The data collected by the axis MEMS gyroscope is fused to complete the attitude calculation independently; the six-axis inertial sensor can be connected with other digital sensors to expand into a nine-axis sensor, which can output a nine-axis signal to establish complete spatial attitude information; The three-axis MEMS gyroscope and the three-axis MEMS accelerometer collect the voltage values of the x-axis, the y-axis and the z-axis respectively, and then convert them into digital signals through the ADC, and finally transmit them to the main control chip through the I2C bus; The formula for converting the above-mentioned quaternion to the inclination of the robot is:

The pitch rotation angle is the required inclination angle of the car; the wireless communication module realizes data communication between the self-balancing robot and external equipment; the control module includes a core controller, and the core controller is a single-chip embedded computer system; An expression is set in the control module, angle: balance angle deviation; Gyro_y: y-axis angular velocity; V: velocity deviation; Vi: velocity deviation integral; Gyro_z: z-axis angular velocity, the expression is:

PWM=angle·Kp+Gyro_y·Kd+V·Kps+Vi·Kis+Gyro_z·Kpt; the critical proportional method is used for parameter setting in the control module.

Embodiment 2:

Referring to Fig. 1, a control method of a self-balancing robot includes an electrical system. The electrical system is divided into a control module, an attitude detection module, a wireless communication module, an execution module, and a power supply module; wherein the power supply module provides stable DC for other parts. The power drive module, the control module sends instructions to control the DC motor in the execution module to run according to the programming method, and the feedback loop is formed by the encoder; the change of the attitude angle in each direction when the system moves is measured by the attitude detection module, and upload the data Processing and analysis, and the data is fed back to the main controller to form a control loop; data communication between the self-balancing robot and external equipment is realized through the wireless communication module.

The attitude detection module adopts the sensor to collect the angular velocity and acceleration signals, directly reads the quaternion and acceleration through the DMP of the sensor, and directly converts the quaternion into the robot inclination, and outputs the voltage and steering applied on the motor through the control module, Control the motor.

The sensor is a six-axis inertial sensor, which includes a three-axis MEMS accelerometer and a three-axis MEMS gyroscope, as well as a digital motion processor DMP, which can process the data collected by the three-axis MEMS accelerometer and the three-axis MEMS gyroscope. Fusion, the attitude calculation is completed independently; and the six-axis inertial sensor can be connected with other digital sensors to expand into a nine-axis sensor, and can output a nine-axis signal to establish complete spatial attitude information. When the chip is working normally, the three-axis MEMS gyroscope and the three-axis MEMS accelerometer collect the voltage values of the x-axis, the y-axis and the z-axis respectively, and then convert them into digital signals through the ADC, and finally transmit them through the I2C bus. To the main control chip, the main control chip communicates with other devices by using 400KHz I2C. A temperature sensor, a 1024-byte FIFO and a high-precision oscillator are embedded in the chip. The DMP digital motion processing engine can convert the gyroscope and The data of the accelerometer is fused and calculated, and the quaternion is directly output. STM32 can obtain the inclination of the robot through simple calculation, so that the main controller does not need to perform the fusion algorithm additionally, and has more time to process various parameters in the control module and motor speed regulation, which reduces the pressure of the main control and improves the real-time performance of the system; the formula for converting the quaternion to the robot inclination is:

A quaternion is a number in the form of ai+bj+ck+d, where a, b, c, and d are real numbers, i, j, and k are imaginary numbers, and the square root of a2+b2+c2+d2 is called a quaternion 's model. definition:

q=[wxyz] ^T

|q| ² =w ² +x ² +y ² +z ² =1;

A quaternion can be constructed from an axis of rotation and an angle around that axis:

w=cos(α/2)

x=sin(α/2)cos(Roll)

y=sin(α/2)cos(Pitch)

z=sin(α/2)cos(Yaw)

Among them, α is the angle around the rotation axis; cos(Roll), cos(Pitch), cos(Yaw) are the components of the rotation axis in the x, y, and z directions;

Quaternion to dip conversion:

Among them, the Pitch rotation angle is the required inclination angle of the self-balancing robot.

Referring to Figure 2, the present invention adopts the digital motion processor inside the MPU6050, and the MPU6050 must be initialized and set up, transferred to the initialization sensor module, and a series of program operations are performed through the control module to perform PWM output or control the motor.

The control module includes a core controller, and the core controller is a single-chip embedded computer system.

Referring to Fig. 3, the expression is set in the control module, angle: balance angle deviation; Gyro_y: y-axis angular velocity; V: velocity deviation; Vi: velocity deviation integral; Gyro_z: z-axis angular velocity, the expression is :

PWM=angle·Kp+Gyro_y·Kd+V·Kps+Vi·Kis+Gyro_z·Kpt.

In the control module, the critical proportional method is used for parameter tuning, and the tuning steps are as follows:

(1) Only the proportional control link is added to the control module, other parameters are set to zero, the proportional gain parameter P value of the main controller is increased, and the output value is observed until the system has a critical oscillation, and the system can be considered to reach a critical state, and the proportion is finally determined. The gain parameter P value is 60%-70% of the current value;

(2) After determining the proportional gain parameter P value, take a larger Ti value, and then gradually reduce the Ti value until the system oscillates. On the contrary, gradually increase the Ti value until the system oscillation disappears, and finally determine the parameter Ti value as 150%-180% of the current value;

(3) The method for determining the value of the parameter differential time constant Td is the same as the method for the value of the proportional gain parameter P, which is 30% of the time when there is no oscillation.

In addition, when the system output does not oscillate, try to increase the proportional gain parameter P value, decrease the integral time constant Ti and increase the differential time constant Td.

In this embodiment, closed-loop control is performed on the person and the control module, and the result is output to the execution module after calculation by the control module.

Embodiment three:

A control method for a self-balancing robot includes an electrical system, providing a stable DC power drive module for other parts through a power module, and the control module sends out instructions to control the DC motor in the execution module to run according to the programming method. Initialization operation, this step is the initialization operation of the entire self-balancing robot, including the initialization of the main controller, sensor initialization, execution module initialization, wireless communication initialization, and interrupt initialization operations. Serial functions, the initialization operation of the wireless communication module can provide guarantee for the communication between the chips, the initialization of the sensor can detect the attitude information more accurately; and the feedback loop is formed by the encoder; the change of the attitude angle in each direction when the system moves is determined by The attitude detection module measures, uploads the data for analysis, and feeds back the data to the main controller to form a control loop; the execution module initialization can initialize the parameters of the motor, especially the initial value of the encoder in the execution module is recorded.

The attitude detection module adopts the sensor to collect the angular velocity and acceleration signals, directly reads the quaternion and acceleration through the DMP of the sensor, and directly converts the quaternion into the robot inclination, and outputs the voltage and steering applied on the motor through the control module, Control the motor. Since general attitude sensors cannot be directly applied to self-balancing control, it is necessary to perform calculations such as filtering and fitting according to the actual sensor selection and system conditions, so as to obtain controllable attitude information transmission and reduce the algorithm pressure of the main controller.

In a specific embodiment, the sensor is a six-axis inertial sensor; the six-axis inertial sensor includes a three-axis MEMS accelerometer and a three-axis MEMS gyroscope, and also includes a digital motion processor DMP, which can convert the three-axis The data collected by the MEMS accelerometer and the three-axis MEMS gyroscope are fused to independently complete the attitude calculation; the six-axis inertial sensor can be connected with other digital sensors to expand into a nine-axis sensor, which can output a nine-axis signal to establish a complete The three-axis MEMS gyroscope and the three-axis MEMS accelerometer collect the voltage values of the x-axis, y-axis and z-axis respectively, and then convert them into digital signals through ADC, and finally transmit them to the I2C bus. Main control chip; the dynamic performance of the gyroscope in the six-axis inertial sensor is better, and the static performance of the accelerometer is better. According to its characteristics, the set value can be designed to obtain the quaternion. The formula for converting the quaternion to the inclination of the robot is:

The pitch rotation angle is the required inclination angle of the car; the wireless communication module realizes data communication between the self-balancing robot and external equipment; the control module includes a core controller, and the core controller is a single-chip embedded computer system;

In a preferred embodiment, the self-balancing robot uses a PID control algorithm to complete the position control function by designing reasonable PID control parameters. PID control consists of proportional unit P, integral unit I and differential unit D. Through the setting of three parameters Kp, Ki and Kd. The PID controller is mainly suitable for systems whose basic linear and dynamic characteristics do not change with time, and is a common feedback loop component in industrial control applications. The controller compares the collected data to a reference value, and then uses the difference to calculate a new input value that is intended to allow the system's data to reach or maintain the reference value. Different from other simple control operations, the PID controller can adjust the input value according to the historical data and the occurrence rate of the difference, which can make the system more accurate and stable. In this embodiment, the critical ratio method is used for parameter tuning, and the tuning steps are as follows:

(1) Only the proportional control link is added to the control module, other parameters are set to zero, the proportional gain parameter P value of the main controller is increased, and the output value is observed until the system has a critical oscillation, and the system can be considered to reach a critical state, and the proportion is finally determined. The gain parameter P value is 60%-70% of the current value;

(2) After determining the proportional gain parameter P value, take a larger Ti value, and then gradually reduce the Ti value until the system oscillates. On the contrary, gradually increase the Ti value until the system oscillation disappears, and finally determine the parameter Ti value as 150%-180% of the current value;

(3) The method for determining the value of the parameter differential time constant Td is the same as the method for the value of the proportional gain parameter P, which is 30% of the time when there is no oscillation.

In addition, when the system output does not oscillate, try to increase the proportional gain parameter P value, decrease the integral time constant Ti and increase the differential time constant Td.

The real-time position information of the self-balancing robot reflected by the encoder in the execution module is used as the feedback quantity of the self-balancing controller, and the control quantity of the self-balancing control is obtained by position iteration, and the difference between the control quantity and the feedback quantity is used as the closed-loop PID control. The input quantity is calculated, and the result is output to the execution module.

Among them, set the expression, angle: balance angle deviation; Gyro_y: y-axis angular velocity; V: velocity deviation; Vi: velocity deviation integral; Gyro_z: z-axis angular velocity, the expression is:

PWM=angle Kp+Gyro_y Kd+V Kps+Vi Kis+Gyro_z Kpt;

The execution module includes a motor, a driving wheel, and an encoder for real-time feedback of the position information of the self-balancing robot. Among them, the motor is connected to the driving wheel, the encoder is connected to the motor, and the encoder converts the angle change generated by the rotation of the motor into its own code number change, which in turn generates the change of the electrical signal, which is used as the position information of the self-balancing robot. The self-balancing position controller is described above, and the encoder has high precision, which is used for real-time and accurate feedback of the position information of the self-balancing robot. Figure 1 has depicted how the encoder and the motor realize information feedback; Figure 2 has depicted The information feedback relationship between the PID control algorithm and other modules in the control module; Fig. 3 has described the specific parameter setting steps of the specific optimized PID control algorithm.

To sum up, the control method of the self-balancing robot of the present invention is simple and convenient, can reduce the algorithm pressure of the main controller, obtain more accurate inclination parameters, and make the self-balancing robot more stable and safe.

While the invention has been described above with reference to various embodiments, it should be understood that many changes and modifications can be made without departing from the scope of the invention. That said, the methods, systems and apparatus discussed above are examples. Various configurations may omit, substitute or add various procedures or components as appropriate. For example, in alternative configurations, the methods may be performed in an order different from that described, and/or various components may be added, omitted, and/or combined. Also features described with respect to certain configurations may be combined in various other configurations, eg, different aspects and elements of the configurations may be combined in a similar manner. Furthermore, elements therein may be updated as technology develops, ie, many of the elements are examples and do not limit the scope of the disclosure or the claims.

Specific details are given in the description to provide a thorough understanding of example configurations, including implementations. However, configurations may be practiced without these specific details. For example, well-known circuits, procedures, algorithms, structures and techniques have been shown without unnecessary detail in order to avoid obscuring the configurations. This description provides example configurations only, and does not limit the scope, applicability, or configuration of the claims. Rather, the foregoing descriptions of configurations will provide those skilled in the art with an enabling description for implementing the described techniques. Various changes may be made in the function and arrangement of elements without departing from the spirit or scope of the disclosure.

In conclusion, it is intended that the foregoing detailed description be regarded as illustrative and not restrictive, and that it should be understood that the following claims, including all equivalents, are intended to define the spirit and scope of the invention. The above embodiments should be understood as only for illustrating the present invention and not for limiting the protection scope of the present invention. After reading the contents of the description of the present invention, the skilled person can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims (10)

1. a kind of control method of self-balance robot, which is characterized in that including electrical system, the electrical system is divided into control Molding block, attitude detection module, wireless communication module, execution module, power module；Wherein power module provides for other parts Stable DC source drive module, control module issue the direct current generator in instruction control execution module according to programming Mode is run, and forms feedback loop by encoder；The variation of all directions attitude angle when system motion, by attitude detection module Measurement, and uploads data processing and is analyzed, while data feedback is to master controller, composition control circuit.

2. the control method of self-balance robot as described in claim 1, which is characterized in that the attitude detection module is adopted With sensor acquisition angle velocity and acceleration signal, quaternary number and acceleration are directly read by the DMP of sensor and by quaternary Number is converted directly into robot inclination angle, and motor is controlled in the voltage being applied on motor by control module output and steering System.

3. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that the sensor For six axis inertial sensors, the six axis inertial sensors include 3 axis MEMS accelerometer and three axis MEMS gyro, are gone back Comprising a digital movement processor DMP, the data that 3 axis MEMS accelerometer and three axis MEMS gyro acquire can be carried out Fusion, complete independently attitude algorithm.

4. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that six axis are used Property sensor can be connect with other digital sensors is extended to nine axle sensors, can export the signal of nine axis, establish Whole spatial attitude information.

5. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that three axis MEMS gyroscope and the 3 axis MEMS accelerometer acquire the voltage value of x-axis, y-axis and z-axis respectively, are then turned by ADC It changes digital signal into, is transmitted to Master control chip finally by I2C bus.

6. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that the quaternary number Be converted to the formula at robot inclination angle are as follows:

Wherein, Pitch rotation angle is exactly the inclination angle for the self-balance robot that necessary requirement obtains.

7. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that pass through wireless telecommunications Module realizes that self-balance robot and external equipment carry out data communication.

8. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that the control mould Block includes core controller, and the core controller is microcontroller embedded computer system.

9. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that the control mould Expression formula is set in block, angle: balance angular deviation；Gyro_y:y axis angular rate；V: velocity deviation；Vi: velocity deviation product Point；Gyro_z:z axis angular rate, the expression formula are as follows:

PWM=angleKp+Gyro_yKd+VKps+ViKis+Gyro_zKpt.

10. the control method of the self-balance robot as described in one of preceding claims, which is characterized in that the control mould Parameter tuning is carried out using aritical ratio method in block.