CN109129481B - Incremental PID (proportion integration differentiation) -based service robot load platform balance control method - Google Patents

Abstract

A service robot load platform balance control method based on incremental PID comprises the following steps: 1) calculating real-time six-axis acceleration of the robot moving platform in the advancing process; 2) performing two-time integration on the acceleration obtained in the last step to obtain coordinate offset of the current four corners of the platform, and simultaneously calculating three-dimensional coordinates of the small balls; 3) performing incremental PID control on four supporting columns of the platform according to the three-dimensional coordinates and the offset obtained by the calculation in the previous step, so that the platform tends to be horizontally stable; 4) projecting the model established in the three-dimensional coordinate system into a two-dimensional plane; 5) matrix operation related in the whole process is simplified by using matlab; 6) the algorithm is embedded into the single chip microcomputer, and the single chip microcomputer controls the extension and contraction of the supporting columns on the periphery of the platform so as to achieve the aim of enabling the platform to tend to be stable. The invention provides a service robot load platform balance control method with strong robustness, high sensitivity and friendly interface.

Description

A load platform balance control method for service robot based on incremental PID

technical field

The invention relates to the fields of balance control, robot, embedded programming and the like. Specifically, it refers to an incremental PID-based load platform balance control method for service robots.

Background technique

In recent years, the design of balance control system has been more and more used in the field of robotics, especially biped robots and self-balancing robots. The market demand is mainly service robots, including social service robots, housekeeping service robots, manufacturing service robots, logistics service robots, etc. Service robots are needed wherever there is service. On the robot mobile platform, it is often necessary to control the tilt angle of the platform according to the application requirements, and to ensure the balance of the platform load. At present, most robot mobile platform systems have high requirements on load weight, placement position, etc., and the robustness is not high; and the stability of the platform during the travel process is mostly guaranteed by the mechanical structure, and there are accumulated errors, which cannot be dynamically compensated in real time. control.

SUMMARY OF THE INVENTION

In order to overcome the shortcomings of low robustness and low sensitivity of the existing service robot load platform balancing control method, the purpose of the present invention is to provide a robust, robust, and stable solution for the instability problem during the traveling process of the above-mentioned service robot mobile platform. A load platform balance control method for service robots with high sensitivity and friendly interface.

In order to solve the above-mentioned technical problems, the technical scheme of the present invention is as follows:

An incremental PID-based load platform balance control method for a service robot, comprising the following steps:

1) Calculate the real-time six-axis acceleration during the movement of the robot mobile platform;

2) Integrate the acceleration obtained in the previous step twice to obtain the coordinate offset of the current four corners of the platform. At the same time, the three-dimensional coordinates of the ball are calculated;

3) According to the three-dimensional coordinates and offsets calculated in the previous step, incremental PID control is performed on the four pillars of the platform, so that the platform tends to be horizontally stable; the four pillars are supported on the four corners of the platform, and the height can be changed to provide Moment against the tilting direction of the platform to bring the system back to the equilibrium position;

4) Project the model established in the three-dimensional coordinate system into the two-dimensional plane;

5) Use matlab to simplify the matrix operations involved in the whole process;

6) Embed the algorithm into the single-chip microcomputer, and the support around the platform is controlled by the single-chip microcomputer to expand and contract to achieve the purpose of making the platform more stable.

Further, in the step 1), the historical data is recorded, the acceleration value of the next state is predicted according to the prediction rule, and after the acceleration value of the next state is obtained by measurement, the two are compared and corrected, and the actual acceleration value is obtained by weighting.

Still further, in the step 3), the corresponding PID parameters are set, and when the platform inclination is large, the correction amount is correspondingly large; when the platform inclination is small, the correction amount also becomes smaller; the user can set it by himself to determine that the platform has reached a stable state. The condition is that the inclination of the platform is within a certain neighborhood of 0° or the load object and the center of the platform.

In described step 4), to project the object from the three-dimensional space coordinate system to the two-dimensional plane coordinate system, first place the three-dimensional object in a coordinate system, and place the midpoint of the plane on the origin of the three-dimensional space; During the process, any flip can be decomposed into rotation around the x, y, and z axes; the size of the field of view is changed by the scaling matrix to change the size of the object on the projection plane; the coordinate position of the object after a series of flips and scaling in the three-dimensional object is given by Matrix operation is obtained.

In the step 6), based on the development of the TivaTM4C1294KCPDT Microcontroller single-chip microcomputer, the construction of the minimum system and the peripheral circuit module is completed.

The technical idea of the present invention is: aiming at the instability problem during the traveling process of the mobile platform of the service robot, with the help of the EK-TM4C1294XL expansion board, the real-time inclination of the extended version is obtained through its own three-axis accelerometer, and the platform is controlled by the incremental PID. The expansion degree of the surrounding pillars makes it tend to be balanced and stable; and the mapping relationship obtained by modeling is reflected on the TFT display screen in real time. Drawing on the processing idea of OpenGL for 3D images, I understand the principle of projecting a 3D image to a 2D image and put it into practice. For the use of various transformations, the matrix operation is simplified by Matlab, an air wall will be added to the TFT display screen to limit the moving range of the ball, the azimuth angle of the platform deviation is calculated by the single-chip microcomputer, and the 3-dimensional gravity-sensing movement of the balance ball is finally realized. The visual interface design is convenient for users to monitor in real time. In addition, this method can freely adjust the allowable tilt degree of the platform by using ADC acquisition and GPIO keys.

The beneficial effects of the invention are shown in: strong robustness, high sensitivity and friendly interface.

Detailed ways

The present invention will be further described below.

A service robot load platform balance control method based on incremental PID, specifically refers to a method based on incremental PID control and Matlab simplified matrix operation to obtain and correct the degree of platform inclination, and display it in EK-based on OpenGL image processing. The method used for real-time observation by the user on the display of TM4C1294XL.

The load platform balance control method for the service robot includes the following steps:

1) Calculate the real-time six-axis acceleration during the movement of the robot mobile platform;

2) Integrate the acceleration obtained in the previous step twice to obtain the coordinate offset of the current four corners of the platform, and calculate the three-dimensional coordinates of the ball at the same time;

3) According to the three-dimensional coordinates and offset calculated in the previous step, incremental PID control is performed on the four pillars of the platform to make the platform tend to be horizontal and stable. The four pillars are supported at the four corners of the platform, and the height can be changed to provide Moment against the tilting direction of the platform to bring the system back to the equilibrium position;

4) Project the model established in the three-dimensional coordinate system into the two-dimensional plane;

5) Use matlab to simplify the matrix operations involved in the whole process, reduce the computational complexity and the required time;

6) Embed the algorithm into the single-chip microcomputer, and the support around the platform is controlled by the single-chip microcomputer to expand and contract to achieve the purpose of making the platform more stable.

Further, in the step 1, the acceleration value is obtained by the MPU6050 accelerometer, and the acceleration value is obtained by multiplying the obtained voltage value by the sensitivity. The user can choose acceleration sensors with different sensitivities and ranges according to actual needs. The data obtained in this way also needs to be filtered by Kalman filtering to remove noise. Record the historical data, predict the acceleration value of the next state according to the prediction rule, and after measuring the acceleration value of the next state, compare and correct the two, and obtain the actual acceleration value by weighting.

In the step 2, the frequency used for accelerometer sampling is obtained by combining the frequency division of the single-chip crystal oscillator, which is converted into a period, and is used as the integration time of each acceleration integration and velocity integration to obtain the offset. The tilt angle of the platform and the platform position where the ball is located are obtained through integration, and these are used as the feedback amount of the PID control.

In the step 3), the corresponding PID parameters are set, and when the platform inclination angle is large, the correction amount is correspondingly large; when the platform inclination angle is small, the correction amount also becomes smaller. Users can set the conditions for judging that the platform has reached a stable state by themselves. It can be that the platform inclination is within a certain neighborhood of 0° or the load object and the center of the platform.

In the step 4), to project the object from the three-dimensional space coordinate system to the two-dimensional plane coordinate system, the three-dimensional object should be placed in a coordinate system first, and the coordinate system (standard right-hand three-dimensional coordinate system) is commonly used when dealing with the above problems. , for the convenience of calculation, place the midpoint of the plane at the origin of the three-dimensional space. During plane flipping, any flip can be decomposed into rotations around the x, y, z axes. The size of the field of view can be changed by the scaling matrix to change the size of the object on the projection plane. The coordinate position of the object in the three-dimensional object after a series of flipping and scaling can be obtained by matrix operation.

In the described step 5), for the matrix that performs graphic transformation, the real-time speed of the single-chip microcomputer cannot meet the system requirements, so the matrix obtained is simplified with unknowns by using Matlab, thereby reducing the calculation time and complexity.

In the step 6), the method is developed based on the TivaTM4C1294KCPDT Microcontroller single-chip microcomputer, completes the construction of the minimum system and the peripheral circuit module, and downloads the project to the single-chip microcomputer through Code Composer Studio 6.0.1, that is, the system can work, and the peripheral circuit Including ADC wheel module, GPIO button module, accelerometer module and TFT liquid crystal display module.

The specific implementation process of the method in this embodiment is as follows: first, the program can be run immediately after being burned into the single-chip microcomputer; the sensitivity and range can be set according to actual requirements; the visual changes, the buttons can simultaneously adjust the scroll wheel to adjust the horizontal viewing angle and the vertical viewing angle. The program will control the expansion and contraction of the support rods around the robot mobile platform according to the incremental PID, so that the platform tends to be balanced and stable.

Claims (5)

1. a service robot load platform balance control method based on incremental PID, is characterized in that, described service robot load platform balance control method comprises the following steps: 1) Calculate the real-time six-axis acceleration during the movement of the robot mobile platform; 2) Integrate the acceleration obtained in the previous step twice to obtain the coordinate offset of the current four corners of the platform, and calculate the three-dimensional coordinates of the ball at the same time; 3) According to the three-dimensional coordinates and offsets calculated in the previous step, incremental PID control is performed on the four pillars of the platform, so that the platform tends to be horizontally stable; the four pillars are supported on the four corners of the platform, and the height can be changed to provide Moment against the tilting direction of the platform to bring the system back to the equilibrium position; 4) Project the model established in the three-dimensional coordinate system into the two-dimensional plane; 5) Use matlab to simplify the matrix operations involved in the whole process; 6) Embed the algorithm into the single-chip microcomputer, and the support around the platform is controlled by the single-chip microcomputer to expand and contract to achieve the purpose of making the platform more stable. 2. a kind of service robot load platform balance control method based on incremental PID as claimed in claim 1, is characterized in that, in described step 1), record historical data, predict the acceleration of next state according to prediction rule After measuring the acceleration value of the next state, the two are compared and corrected, and the actual acceleration value is obtained by weighting. 3. a kind of service robot load platform balance control method based on incremental PID as claimed in claim 1 or 2, is characterized in that, in described step 3), set corresponding PID parameter, when platform inclination angle is large, Make the correction amount correspondingly large; when the platform inclination is small, the correction amount also becomes smaller; the user can set the conditions for judging that the platform has reached a stable state, the conditions are that the platform inclination is within a certain neighborhood of 0° or the load object is adjacent to the center of the platform. within the domain. 4. a kind of service robot load platform balance control method based on incremental PID as claimed in claim 1 or 2, is characterized in that, in described step 4), to project object from three-dimensional space coordinate system to two-dimensional In the plane coordinate system, first place the three-dimensional object in a coordinate system, and place the midpoint of the plane at the origin of the three-dimensional space; in the process of plane flipping, any flip can be decomposed into rotation around the x, y, and z axes; The scaling matrix changes the size of the field of view to change the size of the object on the projection plane; after a series of flipping and scaling, the coordinate position of the object in the three-dimensional object is obtained by the matrix operation. 5. a kind of service robot load platform balance control method based on incremental PID as claimed in claim 1 and 2, is characterized in that, in described step 6), based on Tiva ^TM TM4C1294KCPDT Microcontroller single-chip development, completes the minimum system and the construction of peripheral circuit modules.