CN113894796B - Mechanical arm multi-joint track time synchronization method based on trapezoidal programming - Google Patents
Mechanical arm multi-joint track time synchronization method based on trapezoidal programming Download PDFInfo
- Publication number
- CN113894796B CN113894796B CN202111404660.2A CN202111404660A CN113894796B CN 113894796 B CN113894796 B CN 113894796B CN 202111404660 A CN202111404660 A CN 202111404660A CN 113894796 B CN113894796 B CN 113894796B
- Authority
- CN
- China
- Prior art keywords
- time
- joint
- track
- trapezoidal
- joints
- 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
Images
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
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
Abstract
The invention relates to a manipulator multi-joint track time synchronization method based on trapezoidal programming, which is characterized in that aiming at the initial pose and the final pose of a multi-joint manipulator, an inverse solution method is firstly used for calculating the initial angle sequence and the key angle sequence of each joint, then a classic trapezoidal track planning method is adopted for solving the short-time trapezoidal track parameter of each joint, then the total operation time of each joint is calculated, and the joint with the maximum total operation time is selected from the short-time trapezoidal track parameter; and finally, correcting the short-time trapezoidal tracks of other joints by combining the selected trapezoidal track parameter of the joint with the maximum total running time, and correcting different motion time into the same motion time, thereby achieving the purpose of starting and stopping all joints simultaneously.
Description
Technical Field
The invention relates to the field of mechanical arm control, in particular to a mechanical arm multi-joint track time synchronization method based on trapezoidal programming.
Background
The joint interpolation function is an important function of a control system of the mechanical arm, and has the effect that the mechanical arm runs from an initial pose to an end pose, and a plurality of joints need to run from a specific initial angle to a specific end angle in the running process. As shown in fig. 1 below, 3 joints are taken as an example.
In multi-joint movement of a mechanical arm, the difference value between the initial angle and the final angle of each joint, namely the angle stroke, is generally different, and the joint interpolation function of the mechanical arm requires that each joint is started at the initial angle and stopped at the final angle simultaneously, namely the time is synchronous.
By using a classical trapezoidal trajectory planning algorithm, when planning different angle strokes, the calculated time is different, namely the time of each joint is not synchronous. If the motion of each joint is asynchronous, when the mechanical arm executes a joint interpolation motion instruction, a plurality of joints stop moving at different time, when a debugging person programs the mechanical arm, the debugging person cannot correctly judge whether the motion instruction stops or is about to stop, misjudgment can be caused, under severe conditions, the mechanical arm collides other peripheral equipment, and therefore time synchronization of the joints becomes a basic function in the development of a mechanical arm control system.
Disclosure of Invention
Aiming at the problems in the prior art, the invention aims to provide a mechanical arm multi-joint track time synchronization method based on trapezoidal programming.
In order to achieve the purpose, the invention adopts the technical scheme that:
a mechanical arm multi-joint track time synchronization method based on trapezoidal programming comprises the following steps:
step 1, for a mechanical arm comprising n joints, the initial pose T of the mechanical arm 0 And end point pose T 1 Calculating the initial angle sequence alpha of each joint by using an inverse solution method 1 ,…,α n And end point angle sequence beta 1 ,…,β n Wherein n is greater than 1;
step 2, according to the parameters of each joint obtained by the calculation: the initial angle, the terminal angle, the limiting speed and the limiting acceleration are subjected to a classical trapezoidal track planning method, and then trapezoidal track parameters of each joint are obtained: acceleration time t1 1 ,…,t1 n And constant speed time t2 1 ,…,t2 n And a deceleration time t3 1 ,…,t3 n And can reach the maximum speed wm 1 ,…,wm n Can reach the maximum acceleration alpham 1 ，…,αm n And velocity trajectory type P 1 ,…,P n ；
P 1 ,…,P n Is 2 or 3, and represents that the speed track is a 2-segment track or a 3-segment track; the set acceleration time and deceleration time are the same when the trapezoidal track planning method is used;
step 3, calculating the total movement of each jointLine time, determining the running time T of n joints 1 ,…,T n (ii) a Taking out the total operating time T of n joints 1 ,…,T n Maximum value of (1), denoted as T max Let the number of the joint with the longest total running time be k;
and 4, performing time synchronization on all the trapezoidal locus parameters of the joints:
the short-time trapezoidal trajectory parameters for all joints are input: acceleration time t1 1 ,…,t1 n And constant speed time t2 1 ,…,t2 n And a deceleration time t3 1 ,…,t3 n Maximum achievable velocity wm 1 ,…,wm n Can achieve the maximum acceleration alpham 1 ，…,αm n And velocity trajectory type P 1 ,…,P n ；
Outputting the long-time trapezoidal track parameters of all the joints, namely the corrected parameter values: corrected acceleration time t1' 1 ,…,t1' n And correcting the constant speed time t2' 1 ,…,t2' n And corrected deceleration time t3' 1 ,…,t3' n Corrected maximum achievable speed wm' 1 ,…,wm' n Corrected maximum acceleration α m' 1 ，…,αm' n And speed trajectory type P' 1 ,…,P' n ；
Judging whether each input joint parameter belongs to the kth joint, if so, judging that the long-time trapezoidal track parameter of the joint is equal to the short-time trapezoidal track parameter; if the input joint does not belong to the kth joint, judging the speed track type of the input joint; when the speed track of the input joint is 2-segment type, the long-term track parameters of the input joint are set as follows:
t2' i ＝0；t3' i ＝t1' i ；P' i ＝P i ；
when the speed track of the input joint is in a 3-segment mode, the long-term track parameters of the input joint are set as follows:
Ω 1 ，…,Ω n is an extreme acceleration, which is a preset value;
and 5, inputting the long-time trapezoidal track parameters of all joints obtained in the step 4 into an interpolation module and issuing the long-time trapezoidal track parameters to corresponding motors.
After the scheme is adopted, aiming at the initial pose and the end pose of the multi-joint mechanical arm, the initial angle sequence and the key angle sequence of each joint are calculated by using an inverse solution method, then the short-time trapezoidal track parameters of each joint are calculated by using a classical trapezoidal track planning method, then the total operation time of each joint is calculated, and the joint with the maximum total operation time is selected from the short-time trapezoidal track parameters; and finally, correcting the short-time trapezoidal tracks of other joints by combining the selected trapezoidal track parameter of the joint with the maximum total running time, and correcting different motion time into the same motion time, thereby achieving the purpose of starting and stopping all joints simultaneously.
Drawings
FIG. 1 is a schematic view of the articulation of a multi-joint robotic arm;
FIG. 2 is a flow chart of a time synchronization method of the present invention;
FIG. 3 is a schematic diagram of multi-joint time synchronization (taking a 3-segment as an example);
FIG. 4 is a schematic flow chart of step 2.
Detailed Description
As shown in fig. 2-3, the invention discloses a time synchronization method for multi-joint trajectories of a mechanical arm based on trapezoidal programming, which is performed on the basis of the following premises:
(1) The invention can be used for any multi-joint mechanical arm, and if n joints are assumed, n is more than 1;
(2) The locus of each jointParameters, including extreme speed Ψ 1 ，…,Ψ n Ultimate acceleration omega 1 ，…,Ω n Is a preset value, which is given in advance;
(3) The set acceleration time and deceleration time are the same when the trapezoidal trajectory planning method is used.
Based on the above, the invention specifically comprises the following steps:
step 1, giving an initial pose T of a mechanical arm 0 And end point pose T 1 Calculating the initial angle sequence alpha of each joint by using an inverse solution method 1 ,…,α n And end point angle sequence beta 1 ,…,β n 。
Step 2, according to the parameters of each joint obtained by the calculation: the initial angle, the terminal angle, the limiting speed and the limiting acceleration are subjected to a classical trapezoidal track planning method, and then trapezoidal track parameters of each joint are obtained: acceleration time t1 1 ,…,t1 n And constant speed time t2 1 ,…,t2 n And a deceleration time t3 1 ,…,t3 n Maximum achievable velocity wm 1 ,…,wm n Can achieve the maximum acceleration alpham 1 ，…,αm n And velocity trajectory type P 1 ,…,P n ；
P 1 ,…,P n The speed trajectory can only be 2 or 3, which means that the speed trajectory is a 2-segment trajectory or a 3-segment trajectory. For each joint, the above 6 parameters are determined, and then the trapezoidal locus of one joint is determined, as shown in fig. 4.
Step 3, calculating the total running time of each joint (e.g. ith joint, T) i ＝t1 i +t2 i +t3 i ) Determining the running times T of the n joints 1 ,…,T n (ii) a Taking out the total operating time T of n joints 1 ,…,T n Maximum value of (D), noted as T max Let the number of the joint with the longest total operation time be k.
And 4, performing time synchronization on all the trapezoidal locus parameters of the joints:
input as the short-time trapezoidal trajectory parameter for all joints (i.e., the relationship obtained in step 2)Section parameters): acceleration time t1 1 ,…,t1 n And constant speed time t2 1 ,…,t2 n And a deceleration time t3 1 ,…,t3 n And can reach the maximum speed wm 1 ,…,wm n Can achieve the maximum acceleration alpham 1 ，…,αm n And velocity trajectory type P 1 ,…,P n 。
Outputting the long-time trapezoidal track parameters of all the joints, namely the corrected parameter values: corrected acceleration time t1' 1 ,…,t1' n And correcting the uniform speed time t2' 1 ,…,t2' n And corrected deceleration time t3' 1 ,…,t3' n Corrected maximum achievable speed wm' 1 ,…,wm' n Corrected maximum acceleration α m' 1 ，…,αm' n And speed trajectory type P' 1 ,…,P' n 。
Judging whether each input joint parameter belongs to the kth joint, if so, judging that the long-time trapezoidal track parameter of the joint is equal to the short-time trapezoidal track parameter; if the input joint does not belong to the kth joint, judging the speed track type of the input joint; when the speed track of the input joint is 2-segment type, the long-term track parameters of the input joint are set as follows:
when the speed track of the input joint is in a 3-segment mode, the long-term track parameters of the input joint are set as follows:
the operation of the step is as follows:
and 5, inputting the long-time trapezoidal track parameters of all joints obtained in the step 4 into an interpolation module and issuing the long-time trapezoidal track parameters to corresponding motors.
In conclusion, aiming at the initial pose and the final pose of the multi-joint mechanical arm, the initial angle sequence and the key angle sequence of each joint are calculated by using an inverse solution method, then the short-time trapezoidal track parameters of each joint are calculated by adopting a classical trapezoidal track planning method, then the total operation time of each joint is calculated, and the joint with the maximum total operation time is selected from the total operation time; and finally, correcting the short-time trapezoidal tracks of other joints by combining the selected trapezoidal track parameter of the joint with the maximum total running time, and correcting different motion time into the same motion time, thereby achieving the purpose of starting and stopping all joints simultaneously.
The above description is only exemplary of the present invention and is not intended to limit the technical scope of the present invention, so that any minor modifications, equivalent changes and modifications made to the above exemplary embodiments according to the technical spirit of the present invention are within the technical scope of the present invention.
Claims (1)
1. A mechanical arm multi-joint track time synchronization method based on trapezoidal programming is characterized by comprising the following steps: the method comprises the following steps:
step 1, for a mechanical arm comprising n joints, the initial pose T of the mechanical arm 0 And end position pose T 1 Calculating the initial angle sequence alpha of each joint by using an inverse solution method 1 ,…,α n And end point angle sequence beta 1 ,…,β n Wherein n is greater than 1;
step 2, according to the parameters of each joint obtained by the calculation: after the initial angle, the terminal angle, the limiting speed and the limiting acceleration are processed by a classical trapezoidal track planning method,solving trapezoidal track parameters of each joint: acceleration time t1 1 ,…,t1 n And constant speed time t2 1 ,…,t2 n And a deceleration time t3 1 ,…,t3 n Maximum achievable velocity wm 1 ,…,wm n Can reach the maximum acceleration alpham 1 ，…,αm n And velocity trajectory type P 1 ,…,P n ；
P 1 ,…,P n Is 2 or 3, and represents that the speed track is a 2-segment track or a 3-segment track; the set acceleration time and deceleration time are the same when the trapezoidal track planning method is used;
step 3, calculating the total running time of each joint to obtain the running time T of n joints 1 ,…,T n (ii) a Taking out the total operating time T of n joints 1 ,…,T n Maximum value of (D), noted as T max Let the number of the joint with the longest total running time be k;
step 4, time synchronization processing is carried out on all the trapezoidal locus parameters of the joints:
the short-time trapezoidal trajectory parameters for all joints are input: acceleration time t1 1 ,…,t1 n And constant speed time t2 1 ,…,t2 n And a deceleration time t3 1 ,…,t3 n And can reach the maximum speed wm 1 ,…,wm n Can achieve the maximum acceleration alpham 1 ，…,αm n And velocity trajectory type P 1 ,…,P n ；
Outputting the long-time trapezoidal track parameters of all joints, namely the corrected parameter values: corrected acceleration time t1' 1 ,…,t1' n And correcting the constant speed time t2' 1 ,…,t2' n And corrected deceleration time t3' 1 ,…,t3' n Corrected maximum achievable speed wm' 1 ,…,wm' n Corrected maximum acceleration of α m' 1 ，…,αm' n And velocity trajectory type P' 1 ,…,P' n ；
Judging whether each input joint parameter belongs to the kth joint, if so, judging that the long-time trapezoidal track parameter of the joint is equal to the short-time trapezoidal track parameter; if it isJudging the speed track type of the input joint, wherein the input joint does not belong to the kth joint; when the speed track of the input joint is 2-segment type, the long-term track parameters of the input joint are set as follows:
when the speed track of the input joint is in a 3-segment mode, the long-term track parameters of the input joint are set as follows:
Ω 1 ，…,Ω n is an extreme acceleration, which is a preset value;
and 5, inputting the long-time trapezoidal track parameters of all the joints obtained in the step 4 into an interpolation module and issuing the long-time trapezoidal track parameters to corresponding motors.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111404660.2A CN113894796B (en) | 2021-11-24 | 2021-11-24 | Mechanical arm multi-joint track time synchronization method based on trapezoidal programming |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111404660.2A CN113894796B (en) | 2021-11-24 | 2021-11-24 | Mechanical arm multi-joint track time synchronization method based on trapezoidal programming |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113894796A CN113894796A (en) | 2022-01-07 |
| CN113894796B true CN113894796B (en) | 2022-12-13 |
Family
ID=79195097
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111404660.2A Active CN113894796B (en) | 2021-11-24 | 2021-11-24 | Mechanical arm multi-joint track time synchronization method based on trapezoidal programming |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113894796B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4529921A (en) * | 1982-12-10 | 1985-07-16 | Kabushiki Kaisha Toyota Chuo Kenkyshuo | Tracking control system of multijoint robot |
| JP2013132696A (en) * | 2011-12-26 | 2013-07-08 | Seiko Epson Corp | Arm control device and arm control method |
| JP2013206425A (en) * | 2012-03-29 | 2013-10-07 | Denso Wave Inc | Robot control device |
| CN107368639A (en) * | 2017-07-10 | 2017-11-21 | 深圳市同川科技有限公司 | Speed planning method, apparatus, computer equipment and storage medium |
| CN110497411A (en) * | 2019-08-23 | 2019-11-26 | 华中科技大学 | A kind of industrial robot cooperative motion control method |
| CN111604905A (en) * | 2020-05-26 | 2020-09-01 | 慧灵科技（深圳）有限公司 | Method, device and system for joint trajectory planning and storage medium |
| CN111897216A (en) * | 2020-07-16 | 2020-11-06 | 华中科技大学 | Multi-motion-segment speed planning and interpolation method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102080812B1 (en) * | 2011-09-02 | 2020-02-24 | 브룩스 오토메이션 인코퍼레이티드 | Time-optimal trajectories for robotic transfer devices |
-
2021
- 2021-11-24 CN CN202111404660.2A patent/CN113894796B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4529921A (en) * | 1982-12-10 | 1985-07-16 | Kabushiki Kaisha Toyota Chuo Kenkyshuo | Tracking control system of multijoint robot |
| JP2013132696A (en) * | 2011-12-26 | 2013-07-08 | Seiko Epson Corp | Arm control device and arm control method |
| JP2013206425A (en) * | 2012-03-29 | 2013-10-07 | Denso Wave Inc | Robot control device |
| CN107368639A (en) * | 2017-07-10 | 2017-11-21 | 深圳市同川科技有限公司 | Speed planning method, apparatus, computer equipment and storage medium |
| CN110497411A (en) * | 2019-08-23 | 2019-11-26 | 华中科技大学 | A kind of industrial robot cooperative motion control method |
| CN111604905A (en) * | 2020-05-26 | 2020-09-01 | 慧灵科技（深圳）有限公司 | Method, device and system for joint trajectory planning and storage medium |
| CN111897216A (en) * | 2020-07-16 | 2020-11-06 | 华中科技大学 | Multi-motion-segment speed planning and interpolation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113894796A (en) | 2022-01-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN101402199B (en) | Hand-eye type robot movable target extracting method with low servo accuracy based on visual sensation | |
| TWI704039B (en) | Mechanical arm singular point control method and system | |
| CN106965171B (en) | The robot device for having learning functionality | |
| CN112757306A (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
| JP4494247B2 (en) | Robot control apparatus and control method | |
| CN110653826B (en) | Real-time robot trajectory planning method oriented to conveyor belt target following | |
| JP6386516B2 (en) | Robot device with learning function | |
| CN113601512B (en) | General avoidance method and system for singular points of mechanical arm | |
| CN109834706B (en) | Method and device for avoiding kinematic singularities in robot motion planning | |
| CN114952868B (en) | 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot | |
| CN109108965B (en) | Cartesian space motion prediction method applied to mechanical arm | |
| US20180264647A1 (en) | Processing device, parameter adjusting method, and storage medium | |
| CN115741695A (en) | Attitude track transition planning method for six-axis series industrial robot tail end | |
| CN113084792B (en) | Method for determining singular areas of joints, robot and storage device | |
| CN113894796B (en) | Mechanical arm multi-joint track time synchronization method based on trapezoidal programming | |
| CN110695994B (en) | Finite time planning method for cooperative repetitive motion of double-arm manipulator | |
| Brecher et al. | Towards anthropomorphic movements for industrial robots | |
| CN111283683A (en) | Servo tracking accelerated convergence method for robot visual feature planning track | |
| CN116803635A (en) | Near-end strategy optimization training acceleration method based on Gaussian kernel loss function | |
| CN111515954A (en) | Method for generating high-quality motion path of mechanical arm | |
| CN109773778A (en) | A kind of planing method of industrial robot joint spatial synchronization movement | |
| CN112192575B (en) | Method and device for transition of space trajectories of adjacent joints | |
| JP3204042B2 (en) | Robot trajectory generator | |
| CN111376262A (en) | Robot off-line programming method | |
| JPH09204216A (en) | Acceleration/deceleration control method for articulated 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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Guo Peng Inventor after: Zhang Jian Inventor after: Wu Yongyi Inventor after: Liu Wei Inventor after: Zhou Wen Inventor before: Guo Peng Inventor before: Liu Zhenliang Inventor before: Xie Shuangsheng Inventor before: Huang Pengsheng Inventor before: Pan Tong |
|
| CB03 | Change of inventor or designer information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |