CN109760053B - Dynamic planning speed control method of truss manipulator - Google Patents
Dynamic planning speed control method of truss manipulator Download PDFInfo
- Publication number
- CN109760053B CN109760053B CN201910067158.3A CN201910067158A CN109760053B CN 109760053 B CN109760053 B CN 109760053B CN 201910067158 A CN201910067158 A CN 201910067158A CN 109760053 B CN109760053 B CN 109760053B
- Authority
- CN
- China
- Prior art keywords
- speed
- current
- maximum
- time
- acceleration
- 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 30
- 230000001133 acceleration Effects 0.000 claims abstract description 34
- 230000008569 process Effects 0.000 claims abstract description 6
- 230000003993 interaction Effects 0.000 claims abstract description 4
- 238000004364 calculation method Methods 0.000 claims description 7
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 claims description 3
- 230000001276 controlling effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000007599 discharging Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005553 drilling Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 238000004080 punching Methods 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
Images
Landscapes
- Numerical Control (AREA)
Abstract
A dynamic planning speed control method of a truss manipulator comprises the following steps: 1) the operation speed of the truss manipulator is curvedThe line is divided into three stages; 2) maximum debugging speed of current system
Can be changed in real time through external man-machine interaction equipment; 3) at an initial speed
Accelerating to the maximum speed set by the system by the acceleration a
Then, entering a real-time dynamic planning stage, dividing the time period T into n blocks, and determining the entrance speed of the current speed block
Maximum debugging speed of system
Calculating to obtain the ending speed of the current speed block
Saving the current speed block end speed according to a dynamic programming algorithm
Inlet velocity as next velocity block
The invention introduces a dynamic programming algorithm, reduces the calculated amount and improves the processing efficiency. Meanwhile, the maximum running speed can be changed in the instruction running process, and the debugging efficiency of the user program is greatly improved.
Description
Technical Field
The invention belongs to the field of motion control, relates to a speed control method of a truss manipulator, and particularly relates to a dynamic programming speed control method of the truss manipulator.
Background
The truss manipulator has a very wide application scene in domestic small and medium-sized enterprises at present, and is a common feeding and discharging manipulator, a drilling manipulator, a punching manipulator and the like of a numerical control lathe.
At present, a domestic manipulator system is gradually matured, in a manipulator control system, the speed control of a motor is based on T-shaped curve speed planning, the calculated amount is small, and the manipulator control system is suitable for embedded equipment with small capacity and low performance. However, almost all velocity planning is only applicable to motion trajectory determination (maximum velocity determination, displacement length determination, etc.), without changing any parameters during motion. Therefore, in the actual debugging process, the efficiency is low when the maximum speed is fixed, so that the running speed cannot be manually changed in real time when the user program is adjusted in a combined mode.
Disclosure of Invention
In order to solve the problems that the maximum running speed cannot be changed in real time and the debugging efficiency is low when the existing truss manipulator controller is used for user program joint debugging, the invention provides a dynamic programming speed control method of a truss manipulator, which can realize the real-time change of the maximum running speed and improve the debugging efficiency; and a speed interpolation strategy is adopted, so that the control performance of the real-time speed is improved.
The technical scheme adopted by the invention for solving the technical problems is as follows:
a method for controlling the dynamically planned speed of a truss manipulator, the method comprising the steps of:
1) dividing the operation speed curve of the truss manipulator into three stages, and determining the initial speed V of the systems PEnd velocity Ve pAcceleration a, system set maximum speed
2) Maximum debugging speed of current system
Can be changed in real time through external man-machine interaction equipment;
3) speed control algorithm with initial speed Vs PAcceleration is carried out to the maximum speed set by the system by the acceleration a
After that, the system goes to realIn the dynamic programming stage, the current maximum debugging speed is adjusted according to the period T
Real-time speed adjustment is performed.
Further, in the step 1), the operation speed curve of the truss manipulator is divided into three stages of acceleration, real-time dynamic control and deceleration, a T-shaped acceleration and deceleration mode is adopted in the acceleration stage and the deceleration stage, and the acceleration is a.
Still further, in the step 2), the maximum debugging speed of the current system is set
Equal to the system set maximum speed by default
The system running speed must be guaranteed at the maximum debugging speed
Within, the biggest debugging speed is carried out size control through the pulse hand wheel, and minimum control amplitude is 10%, divide into 10 shelves altogether, do respectively: 10%, 20%, 30%,. 90%, 100%.
Further, in the step 3), the real-time dynamic control stage is divided into n blocks according to the time period T, and the entrance speed V of the current speed block is determineds nMaximum system debug speed
Calculating to obtain the ending speed V of the current speed blocke n(ii) a Saving the current velocity block end velocity V according to a dynamic programming algorithme nAs inlet velocity V of the next velocity blocks nAnd the calculation amount is reduced.
Still further, in the step 3), it is required to determine the inlet speed V of the current speed blocks nMaximum system debug speed
The current speed block is regulated in a T-shaped acceleration and deceleration mode, and the ideal ending speed of the current speed block is calculated according to the formula
Final ending velocity Ve nThe value of (A) is divided into two cases of ① if the result V is calculatede n' greater than current maximum debug speed
Then take the current velocity block end velocity Ve nIs composed of
② if the result V is calculatede n' less than current maximum debug speed
Then take the current velocity block end velocity Ve nTo calculate the result Ve n' saving the current velocity block end velocity V according to a dynamic programming algorithme nAs inlet velocity V of the next velocity blocks nTo reduce the amount of computation.
Still further, in the step 3), the debugging speed is adjusted according to the period T and the current maximum debugging speed
And (4) carrying out real-time speed regulation, wherein the regulation mode is carried out by adopting a T-shaped acceleration and deceleration mode. Real-time judgment of current speed V in dynamic planning stagenowAnd whether the corresponding deceleration distance is enough or not, closing the dynamic programming cycle when the deceleration distance is close to the set threshold value N, and entering a deceleration stage.
In the step 3), in the real-time dynamic planning process, aiming at the calculation result Ve n' greater than current maximum debug speed
The case (2) requires additional processing, first by
Derived out of
t is the actual acceleration time.
In the step 3), by
Calculating the actual acceleration time t according to the pulse period time formula
Obtaining: period of time
TiA timer period formula corresponding to the ith pulse and the period corresponding to the ith pulse
TimprescalerFor clock pre-division of coefficients, arriFor automatic reloading values, the calculated desired speed can be converted into arriThe problems of the judgment of the value of the automatic reloading value, the actual acceleration time T and the period T are all converted into the problem of recording the number of pulses.
In the invention, the truss manipulator control system is controlled by the microcontroller, so that a continuous time model needs to be converted into a discrete model, thereby greatly improving the processing efficiency of the controller and enhancing the real-time property of the system.
The invention has the beneficial effects that: (1) under the condition of debugging a user program, the maximum operation speed can be changed in the operation process of one instruction, so that the debugging efficiency is greatly improved; (2) the running speed is calculated in real time according to the period, the change of the maximum speed can be responded in time, and the required speed is reached in the shortest time; (3) and a dynamic programming algorithm is introduced, so that the calculated amount is reduced, and the processing efficiency is improved.
Drawings
FIG. 1 is a graph of a speed dynamic programming curve with a maximum debug speed increased;
FIG. 2 is a velocity dynamic programming curve with reduced maximum debug velocity;
fig. 3 is a speed curve in which the maximum debugging speed increases twice in the speed dynamic programming period.
Detailed Description
Embodiments of the present invention are further described below with reference to the accompanying drawings.
Referring to fig. 1 to 3, a method for controlling a dynamically planned speed of a truss manipulator includes the steps of:
1) as shown in figure 1, the operation speed curve of the truss manipulator is divided into three stages, and the initial speed V of the system is determineds PEnd velocity Ve pAcceleration a, system set maximum speed
2) Maximum debugging speed of current system
Can be changed in real time through external man-machine interaction equipment;
3) speed control algorithm with initial speed Vs PAcceleration is carried out to the maximum speed set by the system by the acceleration a
Then, the system enters a real-time dynamic planning stage according to the period T and the current maximum debugging speed
Carrying out real-time speed adjustment;
the system running speed must be guaranteed at the maximum debugging speed
In-situ, maximum debug speed is achieved by pulsing the handThe wheel carries out size adjustment, and minimum regulation amplitude is 10%, divides into 10 shelves altogether, is respectively: 10%, 20%, 30%,. 90%, 100%. Fig. 1 shows a speed variation curve in which the maximum debug speed becomes larger, and fig. 2 shows a speed variation curve in which the maximum debug speed becomes smaller;
as shown in FIG. 3, the real-time dynamic speed control stage is divided into n blocks according to the time period T, and the inlet speed V of the current speed block is determineds nMaximum system debug speed
Calculating to obtain the ending speed V of the current speed blocke n. Saving the current velocity block end velocity V according to a dynamic programming algorithme nAs inlet velocity V of the next velocity blocks nAnd the calculation amount is reduced.
The operation speed curve of the truss manipulator obtained according to the step 1) is divided into three stages of acceleration, real-time dynamic planning and deceleration, wherein a T-shaped acceleration and deceleration mode is adopted in the acceleration stage and the deceleration stage, and the acceleration is a.
Further, the speed does not reach the maximum speed set by the system
Previously, the system would not enter the dynamic programming phase.
And further, the system adopts constant acceleration a to carry out acceleration and deceleration control in an acceleration stage, a deceleration stage and a dynamic planning stage.
Further, the current speed V is judged in real time in the dynamic planning stagenAnd whether the corresponding deceleration distance is enough or not, and manually setting the current speed V through earlier stage testnAnd a corresponding deceleration distance table. And judging whether to enter a deceleration stage or not by a table look-up method. The table is only used in the dynamic programming period, and when the remaining non-operating distance is less than or equal to the set threshold value N, the dynamic programming period is closed, and the deceleration stage is started.
Furthermore, in the step 3), the real-time dynamic control stage is a problem with overlapping subproblems and the most substructure property, so a dynamic programming method can be adopted, and the operation time is far shorter than that of a simple solution method. And in the real-time dynamic control stage, the speed blocks with the time period T are divided into n blocks according to the time period T, each block in the n speed blocks with the time period T is used as a speed planning subproblem, the result (ending speed) obtained by calculating the subproblem is stored by solving a simple subproblem, and the result is directly inquired when the next subproblem is solved, so that the operation time is reduced.
According to the step 3), the current speed block adjusting mode is carried out by adopting a T-shaped acceleration and deceleration mode, and the ideal ending speed calculation formula is
Final ending velocity Ve nThe value of (A) is divided into two cases of ① if the result V is calculatede n' greater than current maximum debug speed
Then take the current velocity block end velocity Ve nIs composed of
② if the result V is calculatede n' less than current maximum debug speed
Then take the current velocity block end velocity Ve nTo calculate the result Ve n′。
Further, in the real-time dynamic planning process, aiming at the calculation result Ve n1Greater than the current maximum debug speed
In the case of time, additional processing is required. As shown in FIG. 3, Tn1To Tn2In the period, the initial velocity V is calculateds nWhen the acceleration a starts to accelerate, the acceleration can be accelerated to the current maximum debugging speed at the time t1
First by
Derived out of
t is the actual acceleration time
Further, according to the nth pulse period time formula
Obtaining: period of time
TiThe period corresponding to the ith pulse.
Still further, the period corresponding to the ith pulse can be calculated according to the period of a timer, and the formula of the period of the timer
TimprescalerFor clock pre-division of coefficients, arriIs an auto reload value.
Still further, the calculated desired velocity may be converted to arriAnd automatically reloading the value of the value. Therefore, the judgment of the actual acceleration time T and the judgment of the period T can be converted into the recording problem of the number of the pulses, and the counting value is accumulated only after the timer is interrupted and enters, and the number of the corresponding counted pulses is judged, so that the system burden is reduced.
Claims (8)
1. A dynamic planning speed control method of a truss manipulator is characterized by comprising the following steps: the method comprises the following steps:
1) dividing the operation speed curve of the truss manipulator into three stages, and determining the initial speed V of the systemS PEnd velocity Ve PAcceleration a, system set maximum speed
2) Maximum debugging speed of current system
Can be changed in real time through external man-machine interaction equipment;
3) speed control algorithm with system initial speed Vs PAcceleration is carried out to the maximum speed set by the system by the acceleration a
Then, the system enters a real-time dynamic planning stage according to the period T and the current maximum debugging speed
Real-time speed adjustment is performed.
2. The method of claim 1, wherein the method comprises the steps of: in the step 1), the operation speed curve of the truss manipulator is divided into three stages of acceleration, real-time dynamic planning and deceleration, wherein a T-shaped acceleration and deceleration mode is adopted in the acceleration stage and the deceleration stage, and the acceleration is a.
3. A method of controlling the dynamically planned speed of a truss manipulator as claimed in claim 1 or 2, wherein: in the step 2), the maximum debugging speed of the current system
Equal to the system set maximum speed by default
The system running speed must be guaranteed at the maximum debugging speed
Within, the maximum debugging speed is carried out by the pulse hand wheelAdjusting, wherein the minimum adjusting amplitude is 10%, the total adjusting amplitude is 10 grades, and the minimum adjusting amplitude is respectively: 10%, 20%, 30%, … … 90%, 100%.
4. A method of controlling the dynamically planned speed of a truss manipulator as claimed in claim 1 or 2, wherein: in the step 3), the real-time dynamic control stage is divided into n blocks according to the time period T, and the entrance speed of the current speed block is determined
Maximum debugging speed of system
Calculating to obtain the ending speed V of the current speed blocke nSaving the current velocity block end velocity V according to a dynamic programming algorithme nAs inlet velocity V of the next velocity blocks n。
5. The method of claim 4, wherein the method comprises the steps of: the inlet velocity V of the current velocity block needs to be determineds nMaximum system debug speed
The current speed block regulation mode is carried out by adopting a T-shaped acceleration and deceleration mode, and the ideal ending speed calculation formula is
Final ending velocity Ve nThe value of (A) is divided into two cases of ① if the result V is calculatede n′Greater than the current maximum debug speed
Then take the current velocity block end velocity Ve nIs composed of
② if the result V is calculatede n′Less than current maximum debug speed
Then take the current velocity block end velocity Ve nTo calculate the result Ve n′。
6. The method for controlling the dynamically planned speed of the truss manipulator as claimed in claim 4, wherein in the step 3), the current maximum commissioning speed is adjusted according to the period T
Real-time speed regulation is carried out in a T-shaped acceleration and deceleration mode, and the current speed V is judged in real time in a dynamic planning stagenowAnd whether the corresponding deceleration distance is enough or not, closing the dynamic programming cycle when the deceleration distance is close to the set threshold value N, and entering a deceleration stage.
7. The method as claimed in claim 4, wherein in the step 3), the calculation result V is calculated in a real-time dynamic planning processe n′Greater than the current maximum debug speed
The case (2) requires additional processing, first by
Derived out of
t is the actual acceleration time.
8. A dynamic programming speed control of a truss robot as defined in claim 4The method is characterized in that in the step 3), the method comprises
Calculating the actual acceleration time t according to the pulse period time formula
Obtaining: period of time
TiA timer period formula corresponding to the ith pulse and the period corresponding to the ith pulse
TimprescalerFor clock pre-division of coefficients, arriFor automatic reloading values, the calculated desired speed can be converted into arriThe problems of the judgment of the value of the automatic reloading value, the actual acceleration time T and the period T are all converted into the problem of recording the number of pulses.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910067158.3A CN109760053B (en) | 2019-01-24 | 2019-01-24 | Dynamic planning speed control method of truss manipulator |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910067158.3A CN109760053B (en) | 2019-01-24 | 2019-01-24 | Dynamic planning speed control method of truss manipulator |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109760053A CN109760053A (en) | 2019-05-17 |
| CN109760053B true CN109760053B (en) | 2020-08-18 |
Family
ID=66454205
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910067158.3A Active CN109760053B (en) | 2019-01-24 | 2019-01-24 | Dynamic planning speed control method of truss manipulator |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109760053B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112277273A (en) * | 2020-10-10 | 2021-01-29 | 深圳市山龙智控有限公司 | Advanced speed changing method and device based on mechanical arm of injection molding machine |
| CN115214245B (en) * | 2021-04-21 | 2023-07-25 | 佛山希望数码印刷设备有限公司 | Method and system for performing ink-jet printing by mechanical arm through PLC |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08263128A (en) * | 1995-03-22 | 1996-10-11 | Fanuc Ltd | Method for acceleration and deceleration control at positioning control time of robot |
| CN103135501A (en) * | 2013-02-01 | 2013-06-05 | 北京配天大富精密机械有限公司 | Acceleration and deceleration controlling method based on S-shaped curve and equipment using the same and numerically-controlled machine tool |
| CN103778843A (en) * | 2012-10-25 | 2014-05-07 | 西安航天精密机电研究所 | Industrial robot demonstration and reappearance method |
| CN103801973A (en) * | 2012-11-09 | 2014-05-21 | 沈阳高精数控技术有限公司 | Optimized control method of servo motor based on servo tool rest controller |
| CN106945042A (en) * | 2017-03-29 | 2017-07-14 | 浙江工业大学 | A kind of discrete S types curve speed control method of manipulator |
| CN109240359A (en) * | 2018-08-29 | 2019-01-18 | 浙江工业大学 | A kind of chip mounter head suction nozzle picks and places the speed planning method of element |
-
2019
- 2019-01-24 CN CN201910067158.3A patent/CN109760053B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08263128A (en) * | 1995-03-22 | 1996-10-11 | Fanuc Ltd | Method for acceleration and deceleration control at positioning control time of robot |
| CN103778843A (en) * | 2012-10-25 | 2014-05-07 | 西安航天精密机电研究所 | Industrial robot demonstration and reappearance method |
| CN103801973A (en) * | 2012-11-09 | 2014-05-21 | 沈阳高精数控技术有限公司 | Optimized control method of servo motor based on servo tool rest controller |
| CN103135501A (en) * | 2013-02-01 | 2013-06-05 | 北京配天大富精密机械有限公司 | Acceleration and deceleration controlling method based on S-shaped curve and equipment using the same and numerically-controlled machine tool |
| CN106945042A (en) * | 2017-03-29 | 2017-07-14 | 浙江工业大学 | A kind of discrete S types curve speed control method of manipulator |
| CN109240359A (en) * | 2018-08-29 | 2019-01-18 | 浙江工业大学 | A kind of chip mounter head suction nozzle picks and places the speed planning method of element |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109760053A (en) | 2019-05-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106168790B (en) | S-shaped acceleration and deceleration control method for changing target speed and position on line | |
| CN102540978B (en) | High-speed processing-oriented surface quality preferred spline real-time interpolation method | |
| CN109760053B (en) | Dynamic planning speed control method of truss manipulator | |
| CN105892402B (en) | Mechanical arm point-to-point motion control method | |
| JP6168033B2 (en) | Vehicle equipped with a fuel cell system | |
| CN103185016A (en) | Method and device for adjusting rotating speed of inner fan of air conditioner | |
| CN111711387B (en) | Flexible speed control method for motor of intelligent glove knitting machine | |
| CN107765639B (en) | A kind of rounding error compensating method of S curve acceleration and deceleration | |
| CN112286057A (en) | Coal amount optimizing and predicting control method based on AGC optimization of thermal power plant | |
| AU2015341253A1 (en) | Switch reluctance motor wide speed-regulation range cross-control method | |
| CN108153246B (en) | Parameter self-adaptive S-shaped speed planning interpolation method based on designated speed | |
| CN106887858B (en) | Energy storage system tracking planned output method and device for accessing new energy power generation | |
| CN111727411B (en) | Equipment motion control method, equipment and storage device | |
| CN107844058A (en) | A kind of curve movement Discrete Dynamic Programming method | |
| CN102287572A (en) | Pulse control method of intelligent valve positioning device | |
| CN116300698A (en) | Thread cutting machining method based on dynamic programming | |
| CN102566496B (en) | Feeding speed dynamic real-time look-ahead control method suitable for numerical control device | |
| CN103163838A (en) | Control method for acceleration and deceleration of numerical control machine tool | |
| CN113759697B (en) | Optimization method based on PSO intelligent setting PID controller parameters of belt conveyor | |
| CN103312248A (en) | Method for compensating inflection point error of linear acceleration to deceleration based on DSP (Digital Signal Processor) | |
| CN116330299B (en) | Industrial robot compliant motion control method based on acceleration smoothing | |
| CN111381565B (en) | Acceleration recursive calculation limiting method for cubic polynomial S curve acceleration and deceleration | |
| CN111798060A (en) | Power instruction optimal distribution method based on unit climbing rate estimation | |
| CN110361964B (en) | Parameter setting method for servo drive controller | |
| CN113703433B (en) | Speed planning method and device for motion trail of robot |
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 |