CN113635301B - Six-axis mechanical arm movement speed control improvement method - Google Patents

Six-axis mechanical arm movement speed control improvement method Download PDF

Info

Publication number
CN113635301B
CN113635301B CN202110864094.7A CN202110864094A CN113635301B CN 113635301 B CN113635301 B CN 113635301B CN 202110864094 A CN202110864094 A CN 202110864094A CN 113635301 B CN113635301 B CN 113635301B
Authority
CN
China
Prior art keywords
speed
acceleration
displacement
stage
deceleration
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
Application number
CN202110864094.7A
Other languages
Chinese (zh)
Other versions
CN113635301A (en
Inventor
刘文韬
刘德刚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China University of Geosciences
Original Assignee
China University of Geosciences
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by China University of Geosciences filed Critical China University of Geosciences
Priority to CN202110864094.7A priority Critical patent/CN113635301B/en
Publication of CN113635301A publication Critical patent/CN113635301A/en
Application granted granted Critical
Publication of CN113635301B publication Critical patent/CN113635301B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1628—Programme controls characterised by the control loop
    • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
    • 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/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P70/00—Climate change mitigation technologies in the production process for final industrial or consumer products
    • Y02P70/10—Greenhouse gas [GHG] capture, material saving, heat recovery or other energy efficient measures, e.g. motor control, characterised by manufacturing processes, e.g. for rolling metal or metal working

Abstract

The invention provides a six-axis mechanical arm movement speed control improvement method, which is based onObtaining S-type acceleration and deceleration curve of track operation according to principle of S-type acceleration and deceleration algorithm, obtaining acceleration function, velocity function and displacement function of each stage, and obtaining the acceleration function, velocity function and displacement function of each stage through a max Maximum value of (a) and v max Obtaining the transition point speed and displacement of each stage; according to the displacement s of the transition point where the current displacement value s is located 0 ～s 7 The interval in the process is subjected to stage judgment to obtain current speed values of different stages, and the control precision of the mechanical arm is improved by obtaining the current speed values of the mechanical arm in real time. The invention has the beneficial effects that: the accuracy and the stability of the novel S-shaped acceleration and deceleration algorithm and the speed planning algorithm are improved, the smooth transition of speed and acceleration can be realized, the shaking phenomenon caused by impact is reduced, and the control precision of the six-axis mechanical arm is improved.

Description

Six-axis mechanical arm movement speed control improvement method
Technical Field
The invention relates to the technical field of robots, in particular to an improvement method for controlling the movement speed of a six-axis mechanical arm.
Background
At present, the robot can be divided into two types of series robots and parallel robots according to the composition of a robot link mechanism, in practical application, the series robots are the mainstream in the market, and are particularly popular in the fields with large space requirements such as carrying, stacking, welding and cutting, and the parallel robots are mostly used in occasions with high requirements on positioning accuracy and speed such as medical treatment, assembly and precision machining. With the vigorous development of modern logistics, cutting and assembling industries, the operable space of the robot needs to be enlarged, and the task can be completed in a short time, so that new requirements are provided for the high-speed and high-precision serial robot.
The special space connecting rod structure and the manufacturing process of the serial robot are main sources of precision errors, vibration is one of main factors which seriously affect the motion precision of the mechanical arm, and therefore, the design of a special acceleration and deceleration algorithm to control the stable motion of the mechanical arm based on the mechanical arm is one of core technologies for motion controller development.
In most six-axis mechanical arm control systems with low precision requirements, the traditional trapezoidal or exponential acceleration and deceleration algorithm is still used, the trapezoidal acceleration and deceleration algorithm still has speed sudden change in the acceleration and deceleration process and cannot ensure flexible control of movement, the exponential acceleration and deceleration algorithm still impacts the whole system when the acceleration suddenly changes, so that a mechanical arm tail end actuating mechanism shakes, and the control precision is greatly influenced.
At present, a controller using a traditional S-type acceleration and deceleration algorithm calculates a current speed value based on time scales, and a corresponding speed value is obtained according to a speed function by taking time as a variable, so that the controller is required to have an accurate time concept.
Disclosure of Invention
In order to solve the problems, the invention provides a six-axis mechanical arm movement speed control improvement method, the method is optimized based on the traditional S-shaped acceleration and deceleration algorithm, the algorithm principle is analyzed, the time is changed into the displacement as the reference, a novel S-shaped acceleration and deceleration algorithm is designed, and the whole acceleration and deceleration process is still divided into 7 stages: adding acceleration, uniformly accelerating, decelerating acceleration, uniform speed, accelerating and decelerating, uniformly decelerating and decelerating, eliminating a time variable by a elimination method, substituting displacement as an independent variable into a function, and calculating the corresponding speed of the current system.
The six-axis mechanical arm movement speed control improvement method mainly comprises the following steps:
s1: according to the principle of an S-type acceleration and deceleration algorithm, the running process of a section of track is divided into 7 stages: an acceleration adding section, a uniform acceleration section, an acceleration reducing section, a uniform speed section, an acceleration and deceleration section, a uniform deceleration section and a deceleration reducing section;
s2: obtaining an acceleration function, a speed function and a displacement function of each stage according to an S-shaped acceleration and deceleration curve corresponding to the track running in the step S1, and defining the S-shaped acceleration and deceleration curve, wherein a is the acceleration and a max Indicating maximum acceleration, T i (i =1,2,3 · · 7) represents the duration of the i-th phase, t i (i =0,1,2 · · 7) represents the moment of the transition point of each phase, J is jerk, v represents velocity, s represents displacement;
s3: according to the acceleration curve in the S-shaped acceleration and deceleration curve, combining the acceleration function, the speed function and the displacement function of each stage through a max Maximum value of (a) andv max calculating to obtain the running time of each stage, obtaining the transition point speed of each stage, and further obtaining the transition point displacement of each stage;
s4: according to the displacement s of the transition point where the current displacement value s is located 0 ～s 7 The interval in the period (S) is subjected to stage judgment, and then a current speed value corresponding to the current displacement value s is obtained according to the relation between the displacement and the speed of each stage;
s5: by the method, the current speed value of the mechanical arm is obtained in real time, and the control precision of the mechanical arm is improved.
Further, a max The maximum value of (a) is: a is max ＝min(a max1 ,a max2 )，
Figure GDA0003986927720000021
Figure GDA0003986927720000022
v max The minimum value of (a) is:
Figure GDA0003986927720000023
wherein v is bgn Starting speed, v, for uniform acceleration or uniform deceleration phases end L represents the total displacement of the trajectory for the end speed of the ramp up or ramp down phase.
Further, according to a max Maximum value of (a) and v max The minimum value of (A) is calculated to obtain the running time length T of each stage 1 ～T 7 Then the time t of each transition point can be obtained 0 ～t 7 And combining the speed function to calculate the speed of each transition point:
Figure GDA0003986927720000031
further, according to the displacement function, the displacement of each transition point is obtained from the speed of each transition point:
Figure GDA0003986927720000032
further, the process of obtaining the current speed values of different stages is as follows:
1) And (3) an acceleration stage:
when s is equal to s 0 ,s 1 ]The acceleration phase is then the equation for displacement and velocity:
Figure GDA0003986927720000033
wherein s represents the currently known displacement, v represents the currently sought velocity, the simultaneous above formula utilizes the elimination method to eliminate the time variable t, because it is a unitary cubic equation about t, three solutions will be generated, and according to the formula, the intermediate variables q and p are defined as:
Figure GDA0003986927720000034
Figure GDA0003986927720000035
the judgment is made by calculating Δ:
Figure GDA0003986927720000041
when Δ <0 there are 3 unequally solid roots, respectively:
Figure GDA0003986927720000042
Figure GDA0003986927720000043
Figure GDA0003986927720000044
wherein the content of the first and second substances,
Figure GDA0003986927720000045
when choosing the correct solution, v is excluded first a ，v b ，v c The second is that the instantaneous speed corresponding to s must be within the speed range [ v ] of the adjacent transition point 0 ，v 1 ]Performing the following steps;
2) A uniform acceleration stage:
when s ∈(s) 1 ,s 2 ]In time, the equation for displacement and velocity in the uniform acceleration phase is:
Figure GDA0003986927720000046
because the equation is a linear equation of two-dimentional about t, two solutions of v can be obtained by eliminating the variable t according to the velocity and displacement of each transition point and a formula method:
Figure GDA0003986927720000047
wherein, the positive number solution is the speed value corresponding to the current displacement s;
3) And (3) an acceleration reducing stage:
when s ∈(s) 2 ,s 3 ]The equations for displacement and velocity in the deceleration and acceleration phases are:
Figure GDA0003986927720000048
defining intermediate variables q and p as follows according to the speed and formula of each transition point:
Figure GDA0003986927720000051
Figure GDA0003986927720000052
according to the method for calculating delta, 3 velocity solutions are calculated, a complex solution or a negative solution is excluded, and a velocity range interval is selected to be [ v [ ] 2 ，v 3 ]The solution in (1) is taken as the speed corresponding to the displacement s in the current deceleration and acceleration stage;
4) A uniform speed stage:
when s ∈(s) 3 ,s 4 ]According to v max Minimum value, if there is a uniform speed stage, the speed value should be the corrected maximum value v max ；
5) An acceleration and deceleration stage:
when s ∈(s) 4 ,s 5 ]In time, the acceleration and deceleration stage is as follows with respect to the displacement and velocity equations:
Figure GDA0003986927720000053
because it is a one-dimensional cubic equation for t, three solutions are generated, defining the intermediate variables q and p as:
Figure GDA0003986927720000054
Figure GDA0003986927720000055
according to the method for calculating delta, 3 velocity solutions are calculated, a complex solution or a negative solution is excluded, and a velocity range interval is selected to be [ v [ ] 4 ，v 5 ]The solution in (1) is used as the speed corresponding to the displacement s in the current acceleration and deceleration stage;
6) A uniform deceleration stage:
when s ∈(s) 5 ,s 6 ]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure GDA0003986927720000056
because the equation is a linear equation of two-dimensional about t, two solutions of v can be obtained by eliminating the variable t according to the velocity and displacement of each transition point and a formula method:
Figure GDA0003986927720000057
wherein the positive numerical solution is the speed value corresponding to the current displacement s.
7) And a deceleration stage:
when s ∈(s) 6 ,s 7 ]The equations for displacement and velocity in the deceleration and acceleration phases are:
Figure GDA0003986927720000061
according to the speed and displacement of each transition point of the formula and a formula method, defining intermediate variables q and p as follows:
Figure GDA0003986927720000062
Figure GDA0003986927720000063
according to the method for calculating delta, 3 velocity solutions are calculated, a complex solution or a negative solution is excluded, and a velocity range interval is selected to be [ v [ ] 6 ，v 7 ]The solution in (1) is taken as the speed corresponding to the displacement s in the current deceleration and acceleration stage.
Further, in order to ensure that the mechanical arm is in smooth transition of a multi-section path, when the speed of a transition point is obtained, the instantaneous speed of the transition point is adjusted according to the size of an included angle of an adjacent path, proper speed reduction is carried out when the included angle is an obtuse angle, smooth transition is realized, and quick speed reduction is carried out when the included angle is a small acute angle, so that sudden change of the speed direction at the transition point is prevented.
The technical scheme provided by the invention has the beneficial effects that: in the multi-section continuous path, the accuracy of the novel S-shaped acceleration and deceleration algorithm and the speed planning algorithm can realize the smooth transition of speed and acceleration, the shaking phenomenon caused by impact is reduced on the premise of not increasing the speed loss in the multi-section path transition, the stability of the algorithm is ensured, and the control precision of the six-axis mechanical arm is further improved.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
fig. 1 is a flowchart of an improved method for controlling the movement speed of a six-axis robot arm according to an embodiment of the present invention.
FIG. 2 is a schematic diagram of an S-shaped acceleration/deceleration curve according to an embodiment of the present invention.
FIG. 3 is a schematic diagram of an S-shaped 5-segment acceleration/deceleration curve according to an embodiment of the present invention.
FIG. 4 is a schematic diagram of an S-shaped 6-segment acceleration/deceleration curve in an embodiment of the present invention.
FIG. 5 is a schematic diagram of a 180 degree angled path in an embodiment of the present invention.
FIG. 6 is a schematic illustration of a 10 degree angled path in an embodiment of the present invention.
FIG. 7 is a schematic diagram of two segments and their included angles in an embodiment of the present invention.
FIG. 8 is a schematic illustration of a single path displacement versus velocity curve in an embodiment of the present invention.
Fig. 9 is a schematic diagram of a path traveled by an end effector of a robotic arm in an embodiment of the invention.
FIG. 10 is a graphical illustration of a multi-segment path displacement-velocity curve in an embodiment of the present invention.
Detailed Description
For a more clear understanding of the technical features, objects, and effects of the present invention, embodiments of the present invention will now be described in detail with reference to the accompanying drawings.
The embodiment of the invention provides a six-axis mechanical arm movement speed control improvement method.
Referring to fig. 1, fig. 1 is a flowchart illustrating an improved method for controlling a movement speed of a six-axis robot arm according to an embodiment of the present invention, which includes the following steps:
s1: according to the principle of an S-type acceleration and deceleration algorithm, the operation process of a section of track is divided into 7 stages: an acceleration adding section, a uniform acceleration section, an acceleration reducing section, a constant speed section, an acceleration and deceleration section, a uniform deceleration section and a deceleration reducing section;
s2: obtaining an acceleration function, a speed function and a displacement function of each stage according to an S-shaped acceleration and deceleration curve corresponding to the track running in the step S1;
s3: according to the acceleration curve in the S-shaped acceleration and deceleration curve, combining the acceleration function, the speed function and the displacement function of each stage through a max Maximum value of (a) and v max Calculating to obtain the running time of each stage, obtaining the transition point speed of each stage, and further obtaining the transition point displacement of each stage;
s4: according to the displacement s of the transition point where the current displacement value s is located 0 ～s 7 The interval in the process is subjected to stage judgment, and then a current speed value corresponding to the current displacement value s is obtained according to the relation between the displacement and the speed of each stage;
s5: by the method, the current speed value of the mechanical arm is obtained in real time, and the control precision of the mechanical arm is improved.
As shown in fig. 2, the principle of the S-type acceleration/deceleration algorithm is to divide the running process of a section of track into 7 stages: the acceleration section, the uniform acceleration section, the deceleration section, the uniform speed section, the acceleration and deceleration section, the uniform deceleration section and the deceleration and deceleration section. We define a as acceleration, a max Indicating maximum acceleration, T i (i =1,2,3 · · 7) represents the duration of the i-th phase, t i (i =0,1,2 · 7) represents the moment of the transition point of each phase, J is jerk, v represents velocity, s represents displacement, we can obtain the various curve expressions from fig. 1:
Figure GDA0003986927720000081
the jerk integration for each stage may obtain the acceleration function a (t) for each stage:
Figure GDA0003986927720000082
and integrating the acceleration function in each stage to obtain a speed function v (t):
Figure GDA0003986927720000083
wherein v is bgn Denotes the starting velocity, v end Indicates the termination velocity, v i (i =0,2,3 \ 8230; 7) indicates the instantaneous velocity at each transition point. And finally, integrating the speed function of each stage to obtain a displacement function s (t):
Figure GDA0003986927720000091
wherein s is i (i =1,2,3 \82307; 7) shows the displacement of the respective transition points, L i (1, 2, 3. Cndot. 7) represents the distance of each stage, and the total displacement of the trajectory is represented by L.
As can be seen from the acceleration curve of FIG. 2, the jerk J is a constant, a max As is known, it is therefore possible to obtain an acceleration from 0 to a maximum a max And the phase durations have the following relationship:
Figure GDA0003986927720000092
if there are even acceleration and even deceleration phases, the speed must be from the starting speed v in the first three phases bgn To a maximum value v max Similarly, the speed in the deceleration stage is increased from the maximum value v in the last three stages max Decrease to the end velocity v end Thus, the duration of the ramp-up and ramp-down phases is:
Figure GDA0003986927720000093
Figure GDA0003986927720000094
according to the formula (5), T 1 ＝T 3 And is non-negative if the maximum acceleration a max Too large, no uniform acceleration stage will be needed, at which time the speed curve degenerates to an S-type 5-segment acceleration-deceleration curve, as shown in fig. 3:
therefore, a needs to be adjusted before S-type acceleration and deceleration starts max To its maximum possible value, according to formulae (6) and (7), and T 2 ＝T 4 =0, calculating a max ：
Figure GDA0003986927720000095
Figure GDA0003986927720000096
Taking the minimum of two according to the equations (9) and (10):
a max ＝min(a max1 ,a max2 ) (10)
for the uniform speed stage, if the total displacement distance L is too short, the maximum speed v is not reached in the acceleration process max The deceleration phase is entered, as in fig. 4:
known as T 1 ，T 2 ，T 3 ，T 5 ，T 6 ，T 7 And L, and additionally T 4 =0, substituting equation (4), calculate v max Minimum possible value:
Figure GDA0003986927720000101
according to the formula, the operation time length T of each stage is calculated 1 ～T 7 Then the time t of each transition point can be obtained 0 ～t 7 Then, the speed of each transition point is obtained according to the formula (3):
Figure GDA0003986927720000102
knowing the speed of each transition point, obtaining the displacement of each transition point by substituting formula (4):
Figure GDA0003986927720000103
finally, according to the transition point displacement s of the current displacement value s 0 ～s 7 The interval in (1) is subjected to stage judgment, and the speed calculation algorithms in different stages are as follows:
1) And (3) an acceleration stage:
when s is equal to s 0 ,s 1 ]The acceleration phase obtained according to equations (3) and (4) is then an equation for displacement and velocity:
Figure GDA0003986927720000111
s represents the currently known displacement, v represents the currently sought velocity, and the simultaneous above formula eliminates the time variable t by using a subtractive method, which results in three solutions because it is a one-dimensional cubic equation about t, and according to the formula, intermediate variables q and p are defined as:
Figure GDA0003986927720000112
Figure GDA0003986927720000113
the judgment is made by calculating Δ:
Figure GDA0003986927720000114
Figure GDA0003986927720000115
when Δ <0 there are 3 unequally solid roots, respectively:
Figure GDA0003986927720000116
Figure GDA0003986927720000117
Figure GDA0003986927720000118
wherein
Figure GDA0003986927720000119
When choosing the correct solution, v is excluded first a ，v b ，v c The second is that the instantaneous speed corresponding to s must be within the speed range [ v ] of the adjacent transition point 0 ，v 1 ]In (1).
2) A uniform acceleration stage:
when s ∈(s) 1 ,s 2 ]The equation of the uniform acceleration phase with respect to displacement and velocity obtained from equation (3) and equation (4):
Figure GDA0003986927720000121
because it is a linear equation of two with respect to t, two solutions for v can be obtained by eliminating the variable t according to equation (12), equation (13) and the formula method:
Figure GDA0003986927720000122
the positive numerical solution is the speed value corresponding to the current displacement s.
3) And (3) a deceleration and acceleration stage:
when s ∈(s) 2 ,s 3 ]Then, the equations of the deceleration and acceleration phases with respect to the displacement and velocity are obtained according to equations (3) and (4):
Figure GDA0003986927720000123
according to equation (12) and the formulation, the intermediate variables q and p are defined as:
Figure GDA0003986927720000124
Figure GDA0003986927720000125
calculating 3 velocity solutions according to the formulas (14) to (16), excluding complex or negative number solutions, and selecting the velocity range interval in [ v [ [ v ] 2 ，v 3 ]The solution in (1) is taken as the speed corresponding to the displacement s in the current deceleration and acceleration stage.
4) A uniform speed stage:
when s ∈(s) 3 ,s 4 ]If the constant speed stage exists according to the formula (11), the speed value is the corrected maximum value v max 。
5) An acceleration and deceleration stage:
when s ∈(s) 4 ,s 5 ]Then, the equations of the acceleration and deceleration stages with respect to displacement and velocity are obtained according to equations (3) and (4):
Figure GDA0003986927720000126
because it is a one-dimensional cubic equation for t, three solutions are generated, defining the intermediate variables q and p as:
Figure GDA0003986927720000131
Figure GDA0003986927720000132
calculating 3 velocity solutions according to the formulas (14) to (16), excluding complex or negative solutions, and selecting a velocity range interval in [ v [ [ v ] 4 ，v 5 ]The solution in (b) is used as the speed corresponding to the displacement s in the current deceleration and acceleration stage.
6) And (3) a uniform deceleration stage:
when s ∈(s) 5 ,s 6 ]Then, the equations of the deceleration and acceleration phases with respect to the displacement and velocity are obtained according to equations (3) and (4):
Figure GDA0003986927720000133
because it is a linear equation of two-fold with respect to t, two solutions of v can be obtained by removing the variable t according to equation (12), equation (13) and formula method:
Figure GDA0003986927720000134
the positive numerical solution is the speed value corresponding to the current displacement s.
7) And a deceleration stage:
when s ∈(s) 6 ,s 7 ]Then, equations of the deceleration and acceleration phases with respect to the displacement and the velocity are obtained according to equations (3) and (4):
Figure GDA0003986927720000135
according to equation (12), equation (13) and the formula, intermediate variables q and p are defined as:
Figure GDA0003986927720000136
Figure GDA0003986927720000137
calculating 3 velocity solutions according to the formulas (14) to (16), excluding complex or negative number solutions, and selecting the velocity range interval in [ v [ [ v ] 6 ，v 7 ]The solution in (1) is taken as the speed corresponding to the displacement s in the current deceleration and acceleration stage.
In the actual motion process of the mechanical arm, the motion path of the end-end actuating mechanism is usually a multi-section path which is connected end to end, if each section of path is completely from 0 to v max Then, in an S-type acceleration and deceleration process to 0, the motion efficiency may be greatly reduced, in a scene with dense paths, the robot may spend a large amount of time at a transition point for acceleration and deceleration, for some transition points, for example, two adjacent paths with an included angle θ =180 ° in fig. 5, redundant deceleration and acceleration processing are not required at the transition point, at this time, the speed of the transition point should satisfy v1= v = v2, v represents an instantaneous speed at the transition point, v1 represents a maximum speed of a path before the transition point, and v2 represents a maximum speed of a path after the transition point; for another example, as shown in fig. 6, because the included angle θ =10 ° between the two paths, the speed suddenly changes in the positive and negative directions at the transition point, and the end effector may shake violently during actual operation to cause deviation and affect the accuracy of the path, we want to reduce the instantaneous speed, i.e., v, at the transition point<v1 and v<v2。
The method comprises the steps of adjusting the instantaneous speed of the transition point according to the size of the included angle of the adjacent paths, carrying out proper speed reduction when the included angle is an obtuse angle to realize smooth transition, and carrying out quick speed reduction when the included angle is a small acute angle to prevent the speed direction at the transition point from changing suddenly.
As shown in FIG. 7, AB and BC are two adjacent paths with an angle θ in between. The value of the included angle can pass through the vector
Figure GDA0003986927720000141
And
Figure GDA0003986927720000142
determining, namely:
Figure GDA0003986927720000143
setting the instantaneous speed v of the transition point B according to the magnitude of the theta value B Since we want θ =180 °, i.e. the two paths are parallel, the speed does not decelerate v at point B B ＝v A ＝v C (ii) a When θ =0 °, i.e. the second path is returned, the speed should be reduced to 0 at point B, i.e. v B =0, the speed and the angle of the path should therefore satisfy the following relationship at the transition point:
Figure GDA0003986927720000144
according to the method, an experiment is carried out, the experiment platform adopts a DL-II type six-axis teaching industrial robot developed by the blue science and technology company Limited, and the whole robot system is divided into an upper computer, a lower computer and hardware.
The host computer uses Android industry screen and rocker peripheral hardware as the hardware basis, and the user carries out human-computer interaction through the rocker of host computer and the mode of touch screen, and the next computer uses a friend brilliant science and technology SoC development board, and two cores are mainly integrated to this SoC development board: a FPGA device of a Cyclone V5 CSEMA4U23C6N and a Cortex-A9 ARM core, wherein a Linux operating system is embedded in an ARM layer, and a C/C + + program is developed to carry out communication, algorithm and process control on the whole robot, and is the core of the whole system; the FPGA layer provides various interfaces for data interaction with the hardware layer; and the hardware layer expands the pin address defined in the FPGA and is in butt joint with the peripheral through an expansion board interface.
In order to visualize the novel speed planning algorithm, a python script program is written according to a log file output in the motion of the robot, and simulation verification is carried out on the speed of the robot.
Setting system parameters: the maximum acceleration is 3, and the jerk is 0.5; path parameters: the starting speed is 10, the ending speed is 20, the maximum speed is 40, the path length is 150, and the final obtainable speed profile is shown in fig. 8. In order to visually verify the multi-segment continuous path speed planning algorithm, system parameters are set: maximum acceleration of 300, jerk of 100, path parameters: the maximum speed is 1000, the length of each path is 8000, and the included angle of each path is shown in fig. 9. Teaching robot walking the route of figure 9, through the log of lower computer output, simulate real-time speed, the speed curve is as shown in figure 10, and whole speed curve is smooth clear, and under the too short or too big circumstances of maximum acceleration degree of route, the algorithm can both accurately be adjusted to the speed of transition point also can smooth transition in the middle of every section orbit, under the condition to the route contained angle is littleer and more, speed loss also reduces along with it gradually, thereby further verified the feasibility of algorithm.
The beneficial effects of the invention are: in the multi-section continuous path, the accuracy of the novel S-shaped acceleration and deceleration algorithm and the speed planning algorithm can realize the smooth transition of speed and acceleration, the shaking phenomenon caused by impact is reduced on the premise of not increasing the speed loss in the multi-section path transition, the stability of the algorithm is ensured, and the control precision of the six-axis mechanical arm is further improved. The specific reasons are as follows:
1. the motion flexibility is improved by setting the maximum acceleration and the jerk according to the use scene of the mechanical arm.
2. And judging whether a 7-segment S-shaped acceleration and deceleration curve can be met or not according to the starting and stopping speed, the maximum speed and the path length of the known path, and if not, adjusting the maximum acceleration and the maximum speed to degrade the curve into a 5-segment or 6-segment S-shaped acceleration and deceleration curve.
3. And calculating the moment and speed of each transition point, dividing the path into stages, directly calculating the current speed according to the stage of the current displacement, and driving the motor to move so as to overcome the problem of speed change delay.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and should not be taken as limiting the scope of the present invention, which is intended to cover any modifications, equivalents, improvements, etc. within the spirit and scope of the present invention.

Claims (5)

1. A six-axis mechanical arm movement speed control improvement method is characterized by comprising the following steps: the method comprises the following steps:
s1: according to the principle of an S-type acceleration and deceleration algorithm, the operation process of a section of track is divided into 7 stages: an acceleration adding section, a uniform acceleration section, an acceleration reducing section, a constant speed section, an acceleration and deceleration section, a uniform deceleration section and a deceleration reducing section;
s2: obtaining an acceleration function, a speed function and a displacement function of each stage according to an S-shaped acceleration and deceleration curve corresponding to the track running in the step S1, and defining the S-shaped acceleration and deceleration curve, wherein a is the acceleration and a max Indicating maximum acceleration, T i (i =1,2,3 · · 7) represents the duration of the i-th phase, t i (i =0,1,2 · · 7) represents the moment of the transition point of each phase, J is jerk, v represents velocity, s represents displacement;
s3: according to the acceleration curve in the S-shaped acceleration and deceleration curve, combining the acceleration function, the speed function and the displacement function of each stage through a max Maximum value of (a) and v max Calculating to obtain the running time of each stage, obtaining the transition point speed of each stage, and further obtaining the transition point displacement of each stage;
s4: according to the displacement s of the transition point where the current displacement value s is located 0 ～s 7 The interval in the process is subjected to stage judgment, and then a current speed value corresponding to the current displacement value s is obtained according to the relation between the displacement and the speed of each stage;
the process of obtaining the current speed values of different stages is as follows:
1) And (3) an acceleration stage:
when s is equal to s 0 ,s 1 ]The acceleration phase is then the equation for displacement and velocity:
Figure FDA0003986927710000011
wherein s represents the currently known displacement, v represents the currently sought speed, and the simultaneous above formula utilizes a vanishing method to eliminate the time variable t, and three solutions are generated because of a unitary cubic equation about t, and according to the formula, intermediate variables q and p are defined as:
Figure FDA0003986927710000012
Figure FDA0003986927710000013
the judgment is made by calculating Δ:
Figure FDA0003986927710000014
when Δ <0, there are 3 unequal roots, respectively:
Figure FDA0003986927710000021
Figure FDA0003986927710000022
Figure FDA0003986927710000023
wherein the content of the first and second substances,
Figure FDA0003986927710000024
when choosing the correct solution, v is excluded first a ，v b ，v c The second is that the instantaneous speed corresponding to s must be within the speed range [ v ] of the adjacent transition point 0 ，v 1 ]Performing the following steps;
2) A uniform acceleration stage:
when s ∈(s) 1 ,s 2 ]The equation for displacement and velocity in the uniform acceleration phase is:
Figure FDA0003986927710000025
because the equation is a linear equation of two-dimentional about t, two solutions of v can be obtained by eliminating the variable t according to the velocity and displacement of each transition point and a formula method:
Figure FDA0003986927710000026
wherein, the positive numerical solution is the speed value corresponding to the current displacement s;
3) And (3) a deceleration and acceleration stage:
when s ∈(s) 2 ,s 3 ]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure FDA0003986927710000027
defining intermediate variables q and p as follows according to the speed and formula of each transition point:
Figure FDA0003986927710000028
Figure FDA0003986927710000029
according to the method for calculating delta, 3 velocity solutions are calculated, a complex solution or a negative solution is excluded, and a velocity range interval is selected to be [ v [ ] 2 ，v 3 ]The solution in (1) is taken as the speed corresponding to the displacement s in the current deceleration and acceleration stage;
4) A uniform speed stage:
when s ∈(s) 3 ,s 4 ]According to v max The minimum value is selected from the group consisting of,if the constant speed stage exists, the speed value is the corrected maximum value v max ；
5) An acceleration and deceleration stage:
when s ∈(s) 4 ,s 5 ]In time, the acceleration and deceleration stage is as follows with respect to the displacement and velocity equations:
Figure FDA0003986927710000031
because it is a one-dimensional cubic equation for t, three solutions are generated, defining the intermediate variables q and p as:
Figure FDA0003986927710000032
Figure FDA0003986927710000033
according to the method for calculating delta, 3 velocity solutions are calculated, a complex solution or a negative solution is excluded, and a velocity range interval is selected to be [ v [ ] 4 ，v 5 ]The solution in (1) is used as the speed corresponding to the displacement s in the current acceleration and deceleration stage;
6) And (3) a uniform deceleration stage:
when s ∈(s) 5 ,s 6 ]The equations for displacement and velocity in the deceleration and acceleration phases are:
Figure FDA0003986927710000034
because the equation is a linear equation of two-dimensional about t, two solutions of v can be obtained by eliminating the variable t according to the velocity and displacement of each transition point and a formula method:
Figure FDA0003986927710000035
wherein, the positive numerical solution is the speed value corresponding to the current displacement s;
7) And a deceleration stage:
when s ∈(s) 6 ,s 7 ]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure FDA0003986927710000041
according to the speed and displacement of each transition point of the formula and a formula method, defining intermediate variables q and p as follows:
Figure FDA0003986927710000042
Figure FDA0003986927710000043
according to the method for calculating delta, 3 velocity solutions are calculated, complex number solutions or negative number solutions are excluded, and a velocity range interval is selected to be [ v [ [ v ] v [ ] 6 ，v 7 ]The solution in (1) is used as the speed corresponding to the displacement s in the current acceleration and deceleration stage;
s5: by the method, the current speed value of the mechanical arm is obtained in real time, and the control precision of the mechanical arm is improved.
2. The improvement method for controlling the movement speed of the six-axis mechanical arm as claimed in claim 1, wherein: in step S3, a max The maximum value of (a) is: a is max ＝min(a max1 ,a max2 )，a max1 ＝sgn(a max )·
Figure FDA0003986927710000044
v max The minimum value of (a) is:
Figure FDA0003986927710000045
wherein，v bgn Starting speed, v, for uniform acceleration or uniform deceleration phases end L represents the total displacement of the trajectory for the end speed of the ramp up or ramp down phase.
3. The improvement method for controlling the movement speed of the six-axis mechanical arm as claimed in claim 2, wherein: in step S3, according to a max Maximum value of (a) and v max Calculating to obtain the operation duration T of each stage 1 ～T 7 The time t of each transition point can be obtained 0 ～t 7 And combining the speed function to calculate the speed of each transition point:
Figure FDA0003986927710000051
4. a six-axis robot arm movement speed control improvement method as claimed in claim 3, wherein:
and according to the displacement function, obtaining the displacement of each transition point according to the speed of each transition point:
Figure FDA0003986927710000052
5. a six-axis robot motion speed control improvement method as claimed in claim 3, wherein:
in order to ensure that the mechanical arm is in smooth transition of multiple sections of paths, when the speed of a transition point is obtained, the instantaneous speed of the transition point is adjusted according to the included angle of an adjacent path, proper speed reduction is carried out when the included angle is an obtuse angle, smooth transition is realized, and quick speed reduction is carried out when the included angle is a smaller acute angle, so that the speed direction mutation at the transition point is prevented.
CN202110864094.7A 2021-07-29 2021-07-29 Six-axis mechanical arm movement speed control improvement method Active CN113635301B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110864094.7A CN113635301B (en) 2021-07-29 2021-07-29 Six-axis mechanical arm movement speed control improvement method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110864094.7A CN113635301B (en) 2021-07-29 2021-07-29 Six-axis mechanical arm movement speed control improvement method

Publications (2)

Publication Number Publication Date
CN113635301A CN113635301A (en) 2021-11-12
CN113635301B true CN113635301B (en) 2023-02-28

Family

ID=78418979

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110864094.7A Active CN113635301B (en) 2021-07-29 2021-07-29 Six-axis mechanical arm movement speed control improvement method

Country Status (1)

Country Link
CN (1) CN113635301B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114578809A (en) * 2022-01-13 2022-06-03 达闼机器人股份有限公司 Speed control method and device of movable equipment and movable equipment
CN114310921B (en) * 2022-03-16 2022-06-10 珞石（北京）科技有限公司 Linear transition path generation method with minimum curvature
CN116572257B (en) * 2023-07-13 2023-09-22 湖南尖山智能科技有限责任公司 Mechanical arm control method, device and system, storage medium and mechanical arm

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1971457A (en) * 2005-11-25 2007-05-30 中国科学院沈阳计算技术研究所有限公司 Speed control method used for numerical control machine
CN105892402A (en) * 2016-06-17 2016-08-24 浙江理工大学 Point-to-point motion control method for mechanical arm
WO2017113219A1 (en) * 2015-12-30 2017-07-06 深圳配天智能技术研究院有限公司 Method, apparatus and system for planning bending following trajectory
CN107272598A (en) * 2017-07-14 2017-10-20 福建工程学院 Nurbs curve interpolating method known to a kind of machining path length
CN110109491A (en) * 2019-05-30 2019-08-09 深圳市众为创造科技有限公司 Continuous S curve acceleration and deceleration motion control method, system and electronic equipment
CN112706166A (en) * 2020-12-22 2021-04-27 江苏集萃智能制造技术研究所有限公司 Multi-joint synchronous trajectory planning method based on S-type and trigonometric functions
CN113156894A (en) * 2021-04-07 2021-07-23 东莞普莱信智能技术有限公司 Speed planning method for chip packaging control and chip packaging control device

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1971457A (en) * 2005-11-25 2007-05-30 中国科学院沈阳计算技术研究所有限公司 Speed control method used for numerical control machine
WO2017113219A1 (en) * 2015-12-30 2017-07-06 深圳配天智能技术研究院有限公司 Method, apparatus and system for planning bending following trajectory
CN105892402A (en) * 2016-06-17 2016-08-24 浙江理工大学 Point-to-point motion control method for mechanical arm
CN107272598A (en) * 2017-07-14 2017-10-20 福建工程学院 Nurbs curve interpolating method known to a kind of machining path length
CN110109491A (en) * 2019-05-30 2019-08-09 深圳市众为创造科技有限公司 Continuous S curve acceleration and deceleration motion control method, system and electronic equipment
CN112706166A (en) * 2020-12-22 2021-04-27 江苏集萃智能制造技术研究所有限公司 Multi-joint synchronous trajectory planning method based on S-type and trigonometric functions
CN113156894A (en) * 2021-04-07 2021-07-23 东莞普莱信智能技术有限公司 Speed planning method for chip packaging control and chip packaging control device

Also Published As

Publication number Publication date
CN113635301A (en) 2021-11-12

Similar Documents

Publication Publication Date Title
CN113635301B (en) Six-axis mechanical arm movement speed control improvement method
JP2801528B2 (en) Object driving time reduction method and multi-axis robot system
CN112757306B (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN107263484B (en) Robot joint space point-to-point motion trajectory planning method
CN103080859B (en) Trajectory control device
CN100580592C (en) Filter technique based numerical control system acceleration and deceleration control method
CN110989626B (en) Unmanned aerial vehicle path planning method based on control parameterization
CN106647282A (en) Six-freedom-degree robot track planning method giving consideration to tail end motion error
CN111158318B (en) Flexible acceleration and deceleration planning method for asymmetric quartic curve
CN108189038A (en) A kind of industry six shaft mechanical arm straight-line trajectory method and system for planning of practicality
CN105159306A (en) Four-rotor aircraft sliding-mode control method based on global stability
CN110561438A (en) Industrial robot manpower/position compliance control method based on kinetic parameter identification
CN113221478A (en) Numerical analysis method and system for actively controlling rotor wing
CN114077226A (en) S-shaped curve speed planning method, control terminal and computer readable storage medium
CN110515348B (en) Servo motor model selection method of machine tool
CN113189938A (en) Speed curve planning method for continuous machining path
CN112720472B (en) Robot trajectory planning method and device, storage medium and robot
CN115122329B (en) Convenient industrial robot joint space trajectory planning method
CN116277036A (en) Rapid fault-tolerant vibration suppression control method for flexible-base and flexible-arm space robot
CN114454180B (en) Motion control method and device of mobile robot and mobile robot
CN103901815A (en) Value control apparatus and value control method
CN109002575A (en) A kind of composite rotor craft transition flight scheme control distribution method
CN114296404A (en) Method for calculating machining track speed of five-axis dispensing machine
CN114019911A (en) Curve fitting method based on speed planning
CN110625617A (en) Intelligent robot trajectory planning method

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