CN109129481B - Incremental PID (proportion integration differentiation) -based service robot load platform balance control method - Google Patents
Incremental PID (proportion integration differentiation) -based service robot load platform balance control method Download PDFInfo
- Publication number
- CN109129481B CN109129481B CN201810986431.8A CN201810986431A CN109129481B CN 109129481 B CN109129481 B CN 109129481B CN 201810986431 A CN201810986431 A CN 201810986431A CN 109129481 B CN109129481 B CN 109129481B
- Authority
- CN
- China
- Prior art keywords
- platform
- dimensional
- control method
- balance control
- service robot
- 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
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000010354 integration Effects 0.000 title claims abstract description 8
- 230000004069 differentiation Effects 0.000 title description 3
- 230000001133 acceleration Effects 0.000 claims abstract description 24
- 239000011159 matrix material Substances 0.000 claims abstract description 15
- 230000008569 process Effects 0.000 claims abstract description 13
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 230000008602 contraction Effects 0.000 claims abstract description 5
- 238000012937 correction Methods 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 4
- 230000002093 peripheral effect Effects 0.000 claims description 4
- 230000007306 turnover Effects 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 230000035945 sensitivity Effects 0.000 abstract description 7
- 230000000007 visual effect Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000013078 crystal Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0008—Balancing devices
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
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
Technical Field
The invention relates to the fields of balance control, robots, embedded programming and the like. In particular to a service robot load platform balance control method based on incremental PID.
Background
In recent years, the design of balance control systems has been increasingly applied to the field of robots, especially biped robots and self-balancing robots. The mapping to market demands is mainly service robots, including social service robots, housekeeping service robots, manufacturing service robots, logistics service robots, and the like, which are needed wherever there is a service. The inclination angle of the platform is controlled according to application requirements on the robot moving platform, and the balance of the platform carrying is ensured. Most of the existing robot mobile platform systems have higher requirements on load weight, placement positions and the like, and the robustness is not high; and the stability of the platform is mostly ensured by a mechanical structure in the advancing process, accumulated errors exist, and the compensation control cannot be dynamically carried out in real time.
Disclosure of Invention
Aiming at overcoming the defects of low robustness and low sensitivity of the existing service robot load platform balance control method, the invention aims to provide the service robot load platform balance control method with strong robustness, high sensitivity and friendly interface aiming at the problem of instability in the moving process of the service robot mobile platform.
In order to solve the technical problems, the technical scheme of the invention is as follows:
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) and performing twice integration on the acceleration obtained in the last step to obtain the coordinate offset of the current four corners of the platform. Simultaneously calculating the three-dimensional coordinates of the small ball;
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; the four supporting columns are supported at four corners of the platform, and the height of the four supporting columns can be changed to provide moment opposite to the inclination direction of the platform, so that the system returns to a balance position;
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.
Further, in the step 1), 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 through measurement, the historical data and the acceleration value are compared and corrected, and the actual acceleration value is obtained through weighting.
Further, in the step 3), setting corresponding PID parameters, and when the inclination angle of the platform is large, making the correction amount correspondingly large; when the inclination angle of the platform is small, the correction amount is also reduced; the user can set the condition for judging that the platform has reached the stable state by himself, wherein the condition is that the inclination angle of the platform is within a certain neighborhood of 0 degrees or the inclination angle of the platform is in the center of the platform.
In the step 4), when an object is projected from a three-dimensional space coordinate system to a two-dimensional plane coordinate system, the three-dimensional object is firstly placed in one coordinate system, and the middle point of the plane is placed at the origin of the three-dimensional space; in the process of planar turnover, any turnover can be decomposed into rotation around x, y and z axes; changing the size of the field of view through the telescopic matrix so as to change the size of the object on the projection plane; the coordinate positions of the object in the three-dimensional object after a series of inversions and zooming are obtained by matrix operation.
And in the step 6), the construction of a minimum system and a peripheral circuit module is completed based on the development of a TivatTM 4C1294KCPDT Microcontroller singlechip.
The technical conception of the invention is as follows: aiming at the problem of instability of a service robot moving platform in the advancing process, by means of an EK-TM4C1294XL expansion board, a real-time inclination angle of an expansion board is obtained through a three-axis accelerometer of the expansion board, and the expansion degree of struts around the platform is controlled by an incremental PID to tend to be balanced and stable; the mapping relation obtained through modeling is reflected on a TFT display screen in real time, the principle that the three-dimensional image projects to a two-dimensional image is understood by using the processing idea of OpenGL on the three-dimensional image, various conversion applications are used, matrix operation is simplified through Matlab, an air wall is added in the TFT display screen to limit the moving range of small balls, the azimuth angle of platform deviation is calculated through a single chip microcomputer, the 3-dimensional visual interface design of gravity sensing movement of the balance balls is finally realized, and real-time monitoring of a user is facilitated. In addition, the method can freely adjust the allowable inclination degree of the platform by using ADC acquisition and GPIO keys.
The beneficial effects of the invention are as follows: strong robustness, high sensitivity and friendly interface.
Detailed Description
The present invention is further explained below.
A service robot load platform balance control method based on incremental PID (proportion integration differentiation), in particular to a method for obtaining and correcting the inclination degree of a platform based on incremental PID control and Matlab simplified matrix operation, and displaying the inclination degree on a display screen of EK-TM4C1294XL for real-time observation of a user based on OpenGL image processing.
The service robot load platform balance control method 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) carrying out incremental PID control on four support columns of the platform according to the three-dimensional coordinates and the offset obtained by the previous step, so that the platform tends to be horizontal and stable, wherein the four support columns are supported at four corners of the platform, and the height of the four support columns can be changed to provide a moment opposite to the inclination direction of the platform, so that the system returns to a balance position;
4) projecting the model established in the three-dimensional coordinate system into a two-dimensional plane;
5) matrix operation involved in the whole process is simplified by using matlab, and the calculation complexity and the required time are reduced;
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.
Further, in the step 1, the acceleration value is obtained by an MPU6050 accelerometer, the acceleration value is obtained by multiplying the obtained voltage value by the sensitivity, and a user can select acceleration sensors with different sensitivities and ranges according to actual requirements. The data thus obtained also needs to be filtered by kalman filtering to remove noise. And recording historical data, predicting the acceleration value of the next state according to a prediction rule, comparing and correcting the acceleration value of the next state after the acceleration value of the next state is obtained through measurement, and weighting to obtain the actual acceleration value.
In the step 2, the frequency used for sampling the accelerometer is obtained by combining the frequency division of the single chip microcomputer crystal oscillator, and is converted into a period which is used as the integral time for obtaining the offset by each acceleration integral and velocity integral. The angle of the platform inclination and the platform position where the small ball is located are obtained through integration, and these are used as feedback quantities of PID control.
In the step 3), setting corresponding PID parameters, and enabling the correction quantity to be correspondingly large when the inclination angle of the platform is large; when the inclination angle of the platform is small, the correction amount is also small. The user can set the condition for judging that the platform reaches the stable state. It may be that the inclination of the platform is in the vicinity of 0 ° or that of the load object and in the central region 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 is firstly placed in one coordinate system, the coordinate system (standard right-hand three-dimensional coordinate system) is commonly used when the above problems are handled, and for the sake of convenience of calculation, the middle point of the plane is placed at the origin of the three-dimensional space. During planar flipping, any flipping can be resolved into rotation about the x, y, z axes. The size of the field of view and thus the size of the object on the projection plane can be varied by the scaling matrix. The coordinate positions of the three-dimensional object after a series of flipping and scaling can be obtained by matrix operation.
In the step 5), for the matrix for carrying out the graph transformation, the real-time speed of the single chip microcomputer does not meet the system requirement, so that the matrix obtained by Matlab is used for simplifying the matrix with unknown numbers, and the calculation time and the complexity are reduced.
In the step 6), the method is developed based on a TivatTM 4C1294KCPDT Microcontroller singlechip, the construction of a minimum system and a peripheral circuit module is completed, and the project is downloaded into the singlechip through Code Composer Studio 6.0.1, so that the system can work, and the peripheral circuit comprises an ADC roller module, a GPIO key module, an accelerometer module and a TFT liquid crystal display screen module.
The method of the embodiment is implemented in the following specific steps: the program can be immediately operated after being burnt into the single chip microcomputer, the sensitivity and the range can be set according to actual requirements in advance, the corresponding visual change of the supporting plate along with the change of the angle can be observed through the display screen of the single chip microcomputer, and the keys can simultaneously adjust the visual angle of the roller wheel in the horizontal direction and the visual angle of the roller wheel in the vertical direction. The inside of the program can control the extension and contraction of the supporting rods around the robot moving 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 by comprising 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; the four supporting columns are supported at four corners of the platform, and the height of the four supporting columns can be changed to provide moment opposite to the inclination direction of the platform, so that the system returns to a balance position;
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.
2. The incremental PID-based service robot load platform balance control method as claimed in claim 1, wherein in step 1), 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 measured, the acceleration value and the next state are compared and corrected, and the actual acceleration value is obtained by weighting.
3. The incremental PID-based service robot load platform balance control method as claimed in claim 1 or 2, wherein in step 3), corresponding PID parameters are set, and when the platform inclination angle is large, the correction amount is made to be correspondingly large; when the inclination angle of the platform is small, the correction amount is also reduced; the user can set the condition for judging that the platform reaches the stable state by himself, wherein the condition is that the inclination angle of the platform is in a certain neighborhood of 0 degrees or the load object is in the neighborhood of the center of the platform.
4. The incremental PID-based service robot load platform balance control method as claimed in claim 1 or 2, wherein 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 is first placed in one coordinate system, and the middle point of the plane is placed at the origin of the three-dimensional space; in the process of planar turnover, any turnover can be decomposed into rotation around x, y and z axes; changing the size of the field of view through the telescopic matrix so as to change the size of the object on the projection plane; the coordinate positions of the object in the three-dimensional object after a series of inversions and zooming are obtained by matrix operation.
5. The incremental PID-based service robot load platform balance control method as claimed in claim 1 or 2, wherein in the step 6), based on TivaTMAnd the TM4C1294KCPDT Microcontroller singlechip is developed to complete the construction of a minimum system and a peripheral circuit module.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810986431.8A CN109129481B (en) | 2018-08-28 | 2018-08-28 | Incremental PID (proportion integration differentiation) -based service robot load platform balance control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810986431.8A CN109129481B (en) | 2018-08-28 | 2018-08-28 | Incremental PID (proportion integration differentiation) -based service robot load platform balance control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109129481A CN109129481A (en) | 2019-01-04 |
| CN109129481B true CN109129481B (en) | 2020-06-30 |
Family
ID=64828458
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810986431.8A Active CN109129481B (en) | 2018-08-28 | 2018-08-28 | Incremental PID (proportion integration differentiation) -based service robot load platform balance control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109129481B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN204101063U (en) * | 2014-09-17 | 2015-01-14 | 阜阳师范学院 | Self-adjusting balance testing table |
| CN106426027A (en) * | 2016-07-26 | 2017-02-22 | 五邑大学 | Multi-motor intelligent lifting worktable control system and method |
| CN107168186A (en) * | 2017-07-05 | 2017-09-15 | 山东科技大学 | 4 automatic horizontal control systems and its method of work based on six axle combination sensors |
| US9975747B1 (en) * | 2014-10-13 | 2018-05-22 | Bill J. Williams | Jack with floating platform |
| CN108224017A (en) * | 2018-02-05 | 2018-06-29 | 上海海事大学 | 3 platform structures and its control method with level-regulating system |
-
2018
- 2018-08-28 CN CN201810986431.8A patent/CN109129481B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN204101063U (en) * | 2014-09-17 | 2015-01-14 | 阜阳师范学院 | Self-adjusting balance testing table |
| US9975747B1 (en) * | 2014-10-13 | 2018-05-22 | Bill J. Williams | Jack with floating platform |
| CN106426027A (en) * | 2016-07-26 | 2017-02-22 | 五邑大学 | Multi-motor intelligent lifting worktable control system and method |
| CN107168186A (en) * | 2017-07-05 | 2017-09-15 | 山东科技大学 | 4 automatic horizontal control systems and its method of work based on six axle combination sensors |
| CN108224017A (en) * | 2018-02-05 | 2018-06-29 | 上海海事大学 | 3 platform structures and its control method with level-regulating system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109129481A (en) | 2019-01-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| RU2664257C2 (en) | Systems and methods for tracking location of mobile target objects | |
| US9989973B2 (en) | Measurement control system for multi-shaft supported air floatation platform | |
| CN102608351B (en) | Detection method and system of three-dimensional gesture of mechanical arm and system controlling mechanical arm to operate | |
| CN109459585B (en) | Accelerometer zero offset correction method | |
| CN103743378B (en) | A kind of gesture detection system of pipeline detector | |
| CN101995949A (en) | Operation input system, control apparatus, handheld apparatus, and operation input method | |
| CN102345390A (en) | Method for compensating for deflection of concrete pump truck arm support | |
| CN1892232A (en) | Electronic apparatus and method of correcting offset value of acceleration sensor | |
| US20190120627A1 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
| CN110986928B (en) | Real-time drift correction method for triaxial gyroscope of photoelectric pod | |
| KR20150106241A (en) | Apparatus and method for controling direction error of gimbal apparatus using image processing | |
| CN112954585B (en) | UWB-based agricultural machine field positioning system and method | |
| CN109129481B (en) | Incremental PID (proportion integration differentiation) -based service robot load platform balance control method | |
| CN210293237U (en) | High-precision attitude measurement system for small satellite onboard equipment | |
| CN111998870B (en) | Calibration method and device of camera inertial navigation system | |
| JP7518975B2 (en) | Motion control method, device, wheel robot, and program | |
| CN103292827A (en) | Data correction method and electronic equipment | |
| CN114660927A (en) | Control method and device of photoelectric pod | |
| CN117288187B (en) | Robot pose determining method and device, electronic equipment and storage medium | |
| CN110705120B (en) | Gravity center position dynamic calculation method of tunnel trackless self-propelled variable-mass platform truck | |
| JP2007327862A (en) | Surveying device | |
| CN111998871A (en) | Drift detection method and system of VR equipment | |
| CN103034345B (en) | Geographical virtual emulation 3D mouse pen in a kind of real space | |
| CN106629394A (en) | Camera external parameter calibration system and method applied to rail-mounted gantry crane hanger pose detection | |
| CN111844013A (en) | Robot gait planning method and device, robot and storage medium |
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 |