CN113635301A - 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
CN113635301A
CN113635301A CN202110864094.7A CN202110864094A CN113635301A CN 113635301 A CN113635301 A CN 113635301A CN 202110864094 A CN202110864094 A CN 202110864094A CN 113635301 A CN113635301 A CN 113635301A
Authority
CN
China
Prior art keywords
speed
acceleration
displacement
deceleration
stage
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.)
Granted
Application number
CN202110864094.7A
Other languages
Chinese (zh)
Other versions
CN113635301B (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

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

The invention provides a six-axis mechanical arm movement speed control improvement method, which comprises the steps of obtaining an S-shaped acceleration and deceleration curve of track running according to the principle of an S-shaped acceleration and deceleration algorithm, obtaining an acceleration function, a speed function and a displacement function of each stage, and obtaining the acceleration function, the speed function and the displacement function of each stage through amaxMaximum value of (a) and vmaxObtaining 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 located0～s7The 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 a six-axis mechanical arm movement speed control improvement method.
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 series 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 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 velocity function and a displacement function of each stage according to the S-shaped acceleration and deceleration curve corresponding to the track running in the step S1, and defining a in the S-shaped acceleration and deceleration curve as acceleration and amaxIndicating maximum acceleration, Ti(i ═ 1,2,3 ·,7) denotes the duration of the i-th phase, ti(i ═ 0,1,2 · 7) denotes the time of the transition point of each stage, J denotes jerk, v denotes velocity, s denotes 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 amaxMaximum value of (a) and vmaxCalculating 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 located0～s7The 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.
Further, amaxThe maximum value of (a) is: a ismax＝min(amax1,amax2)，
Figure BDA0003186713880000021
Figure BDA0003186713880000022
vmaxThe minimum value of (a) is:
Figure BDA0003186713880000023
wherein v isbgnStarting speed, v, for uniform acceleration or uniform deceleration phasesendL represents the total displacement of the trajectory for the end speed of the uniform acceleration or uniform deceleration phase.
Go toStep a, according tomaxMaximum value of (a) and vmaxThe minimum value of (A) is calculated to obtain the running time length T of each stage1～T7Then the time t of each transition point can be obtained0～t7And combining the speed function to calculate the speed of each transition point:
Figure BDA0003186713880000031
further, according to the displacement function, the displacement of each transition point is obtained from the speed of each transition point:
Figure BDA0003186713880000032
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 s0,s1]The acceleration phase is then the equation for displacement and velocity:
Figure BDA0003186713880000033
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 BDA0003186713880000034
Figure BDA0003186713880000035
the judgment is made by calculating Δ:
Figure BDA0003186713880000041
when Δ <0 there are 3 unequally solid roots, respectively:
Figure BDA0003186713880000042
Figure BDA0003186713880000043
Figure BDA0003186713880000044
wherein the content of the first and second substances,
Figure BDA0003186713880000045
when choosing the correct solution, v is excluded firsta，vb，vcThe second is that the instantaneous speed corresponding to s must be within the speed range [ v ] of the adjacent transition point0，v1]Performing the following steps;
2) a uniform acceleration stage:
when s ∈(s)1,s2]The equation for displacement and velocity in the uniform acceleration phase is:
Figure BDA0003186713880000046
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 BDA0003186713880000047
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,s3]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure BDA0003186713880000048
defining intermediate variables q and p as follows according to the speed and formula of each transition point:
Figure BDA0003186713880000051
Figure BDA0003186713880000052
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，v3]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,s4]According to vmaxMinimum value, if there is a uniform speed stage, the speed value should be the maximum value v after correctionmax；
5) An acceleration and deceleration stage:
when s ∈(s)4,s5]In time, the acceleration and deceleration stage is as follows with respect to the displacement and velocity equations:
Figure BDA0003186713880000053
because it is a one-dimensional cubic equation for t, three solutions are generated, defining the intermediate variables q and p as:
Figure BDA0003186713880000054
Figure BDA0003186713880000055
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，v5]The solution in (1) is taken 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,s6]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure BDA0003186713880000056
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 BDA0003186713880000057
the positive numerical solution is the speed value corresponding to the current displacement s.
7) And a deceleration stage:
when s ∈(s)6,s7]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure BDA0003186713880000061
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 BDA0003186713880000062
Figure BDA0003186713880000063
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，v7]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 transition of the multi-section path, 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 improving 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 the 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 amaxMaximum value of (a) and vmaxCalculating 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 located0～s7The 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 operation process of a section of track into 7 stages: adding an accelerating section and uniformly addingThe device comprises a speed section, an acceleration reducing section, a constant speed section, an acceleration and deceleration section, a uniform deceleration section and a deceleration reducing section. We define a as acceleration, amaxIndicating maximum acceleration, Ti(i ═ 1,2,3 ·,7) denotes the duration of the i-th phase, ti(i ═ 0,1,2 · 7) denotes the moment of the transition point of each phase, J denotes jerk, v denotes velocity, s denotes displacement, we can obtain the various curve expressions from fig. 1:
Figure BDA0003186713880000081
the jerk integration for each stage may obtain the acceleration function a (t) for each stage:
Figure BDA0003186713880000082
the velocity function v (t) can be obtained by integrating the acceleration function in each stage:
Figure BDA0003186713880000083
wherein v isbgnDenotes the starting velocity, vendIndicates the termination velocity, vi(i ═ 0,2,3 … 7) represents the instantaneous velocity at each transition point. And finally, integrating the speed function of each stage to obtain a displacement function s (t):
Figure BDA0003186713880000091
wherein s isi(i ═ 1,2,3 … 7) denotes the displacement of the respective transition points, denoted Li(1,2,3 … 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, amaxAs is known, it is therefore possible to obtain an acceleration from 0 to a maximum amaxAnd the duration of each stage has the following relationship：
Figure BDA0003186713880000092
If there are even acceleration and even deceleration phases, the speed must be from the starting speed v in the first three phasesbgnTo a maximum value vmaxSimilarly, the speed in the deceleration stage is increased from the maximum value v in the last three stagesmaxDecrease to the end velocity vendThus, the duration of the ramp-up and ramp-down phases is:
Figure BDA0003186713880000093
Figure BDA0003186713880000094
according to the formula (5), T1＝T3And is non-negative if the maximum acceleration amaxToo 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, it is necessary to adjust a before starting S-type acceleration/decelerationmaxTo its maximum possible value, according to formulae (6) and (7), and T2＝T4When it is 0, calculate amax：
Figure BDA0003186713880000095
Figure BDA0003186713880000096
Taking the minimum of two according to the equations (9) and (10):
amax＝min(amax1,amax2) (10)
for the uniform speed stage, if the total displacement distance L is too short, the acceleration process will not be performed yetReaches a maximum value v of speedmaxThe deceleration phase is entered, as in fig. 4:
known as T1，T2，T3，T5，T6，T7And L, and additionally T4When 0, formula (4) is substituted, and v is calculatedmaxMinimum possible:
Figure BDA0003186713880000101
according to the formula, the operation time length T of each stage is calculated1～T7Then the time t of each transition point can be obtained0～t7Then, the speed of each transition point is obtained according to the formula (3):
Figure BDA0003186713880000102
knowing the speed of each transition point, obtaining the displacement of each transition point by substituting formula (4):
Figure BDA0003186713880000103
finally, according to the transition point displacement s of the current displacement value s0～s7The 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 s0,s1]The acceleration phase obtained according to equations (3) and (4) is then an equation for displacement and velocity:
Figure BDA0003186713880000111
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 BDA0003186713880000112
Figure BDA0003186713880000113
the judgment is made by calculating Δ:
Figure BDA0003186713880000114
Figure BDA0003186713880000115
when Δ <0 there are 3 unequally solid roots, respectively:
Figure BDA0003186713880000116
Figure BDA0003186713880000117
Figure BDA0003186713880000118
wherein
Figure BDA0003186713880000119
When choosing the correct solution, v is excluded firsta，vb，vcThe second is that the instantaneous speed corresponding to s must be within the speed range [ v ] of the adjacent transition point0，v1]In (1).
2) A uniform acceleration stage:
when s ∈(s)1,s2]Then, the equation of the uniform acceleration phase with respect to displacement and velocity obtained from equations (3) and (4):
Figure BDA0003186713880000121
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 BDA0003186713880000122
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,s3]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 BDA0003186713880000123
according to equation (12) and the formulation, the intermediate variables q and p are defined as:
Figure BDA0003186713880000124
Figure BDA0003186713880000125
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 ]2，v3]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,s4]If the constant speed stage exists according to the formula (11), the speed value is the corrected maximum value vmax。
5) An acceleration and deceleration stage:
when s ∈(s)4,s5]Then, the equations of the acceleration and deceleration stages with respect to displacement and velocity are obtained according to equations (3) and (4):
Figure BDA0003186713880000126
because it is a one-dimensional cubic equation for t, three solutions are generated, defining the intermediate variables q and p as:
Figure BDA0003186713880000131
Figure BDA0003186713880000132
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，v5]The solution in (1) is taken 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,s6]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 BDA0003186713880000133
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 BDA0003186713880000134
the positive numerical solution is the speed value corresponding to the current displacement s.
7) And a deceleration stage:
when s ∈(s)6,s7]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 BDA0003186713880000135
according to equation (12), equation (13) and the formula, intermediate variables q and p are defined as:
Figure BDA0003186713880000136
Figure BDA0003186713880000137
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 ]6，v7]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 vmaxIn a scene with dense paths, the robot may spend a lot of time at a transition point for acceleration and deceleration, for some transition points, for example, two adjacent paths with an included angle θ of 180 ° in fig. 5, the transition point does not need to perform redundant deceleration and acceleration processing, and at this time, the speed of the transition point should satisfy v1 v2, v represents the instantaneous speed at the transition point, v1 represents the maximum speed of the path before the transition point, and v2 represents the maximum speed of the path after the transition point; for another example, as shown in fig. 6, because the included angle θ between the two paths is 10 °, the speed abruptly changes in the positive and negative directions at the transition point, the end effector may shake sharply during actual operation to cause deviation, and the accuracy of the path is affected, it is desirable 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 BDA0003186713880000141
And
Figure BDA0003186713880000142
determining, namely:
Figure BDA0003186713880000143
setting the instantaneous speed v of the transition point B according to the magnitude of the theta valueBSince we want θ to be 180 °, i.e. the two paths are parallel, the speed does not decelerate v at point BB＝vA＝vC(ii) a When θ is 0 °, i.e. the second path is returned, the speed should be reduced to 0 at point B, i.e. v B0, the velocity and the angle of the path should therefore satisfy the following relationship at the transition point:
Figure BDA0003186713880000144
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: an FPGA device of a Cyclone V5 CSEMA4U23C6N and a Cortex-A9 ARM core are embedded into an ARM layer, a Linux operating system is developed, a C/C + + program is developed to carry out communication, algorithm and process control on the whole robot, and the C/C + + program 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 the speed of the robot is subjected to simulation verification.
Setting system parameters: the maximum acceleration is 3, and the jerk is 0.5; path parameters: the starting speed was 10, the ending speed was 20, the maximum speed was 40, the path length was 150, and the final obtainable speed profile is shown in fig. 8. In order to visually verify the multi-section continuous path speed planning algorithm, system parameters are set as follows: 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 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 transition of the multi-section path, 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 the 7-segment S-shaped acceleration and deceleration curve can be met or not according to the known starting and stopping speed, the maximum speed and the path length of the one-segment path, and if not, adjusting the maximum acceleration and the maximum speed to degrade the maximum acceleration and the maximum speed into the 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 is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (6)

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 velocity function and a displacement function of each stage according to the S-shaped acceleration and deceleration curve corresponding to the track running in the step S1, and defining a in the S-shaped acceleration and deceleration curve as acceleration and amaxIndicating maximum acceleration, Ti(i ═ 1,2,3 ·,7) denotes the duration of the i-th phase, ti(i ═ 0,1,2 · 7) denotes the time of the transition point of each stage, J denotes jerk, v denotes velocity, s denotes 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 amaxMaximum value of (a) and vmaxCalculating 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 located0～s7The interval in (1) is subjected to stage judgment, and then the current displacement value s corresponding to the current displacement value s is obtained according to the relation between the displacement and the speed of each stageA front velocity value;
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, amaxThe maximum value of (a) is: a ismax＝min(amax1,amax2)，amax1＝sgn(amax)·
Figure FDA0003186713870000011
vmaxThe minimum value of (a) is:
Figure FDA0003186713870000012
wherein v isbhnStarting speed, v, for uniform acceleration or uniform deceleration phasesendL represents the total displacement of the trajectory for the end speed of the uniform acceleration or uniform deceleration 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 amaxMaximum value of (a) and vmaxThe minimum value of (A) is calculated to obtain the running time length T of each stage1～T7Then the time t of each transition point can be obtained0～t7And combining the speed function to calculate the speed of each transition point:
Figure FDA0003186713870000021
4. a six-axis robot motion 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 FDA0003186713870000022
5. the improvement method for controlling the movement speed of the six-axis mechanical arm as claimed in claim 1, wherein: in step S4, 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 s0,s1]The acceleration phase is then the equation for displacement and velocity:
Figure FDA0003186713870000023
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 FDA0003186713870000024
Figure FDA0003186713870000025
the judgment is made by calculating Δ:
Figure FDA0003186713870000031
when Δ <0 there are 3 unequally solid roots, respectively:
Figure FDA0003186713870000032
Figure FDA0003186713870000033
Figure FDA0003186713870000034
wherein the content of the first and second substances,
Figure FDA0003186713870000035
when choosing the correct solution, v is excluded firsta，vb，vcThe second is that the instantaneous speed corresponding to s must be within the speed range [ v ] of the adjacent transition point0，v1]Performing the following steps;
2) a uniform acceleration stage:
when s ∈(s)1,s2]The equation for displacement and velocity in the uniform acceleration phase is:
Figure FDA0003186713870000036
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 FDA0003186713870000037
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,s3]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure FDA0003186713870000038
defining intermediate variables q and p as follows according to the speed and formula of each transition point:
Figure FDA0003186713870000041
Figure FDA0003186713870000042
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，v3]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,s4]According to vmaxMinimum value, if there is a uniform speed stage, the speed value should be the maximum value v after correctionmax；
5) An acceleration and deceleration stage:
when s ∈(s)4,s5]In time, the acceleration and deceleration stage is as follows with respect to the displacement and velocity equations:
Figure FDA0003186713870000043
because it is a one-dimensional cubic equation for t, three solutions are generated, defining the intermediate variables q and p as:
Figure FDA0003186713870000044
Figure FDA0003186713870000045
according to the method for calculating delta, 3 are calculatedVelocity solution, excluding complex or negative solutions, selecting a velocity range interval in [ v ]4，v5]The solution in (1) is taken 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,s6]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure FDA0003186713870000046
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 FDA0003186713870000047
the positive numerical solution is the speed value corresponding to the current displacement s.
7) And a deceleration stage:
when s ∈(s)6,s7]The equations for the displacement and velocity in the deceleration and acceleration phases are:
Figure FDA0003186713870000051
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 FDA0003186713870000052
Figure FDA0003186713870000053
according to the method for calculating delta, 3 velocity solutions are calculated, the complex number solution or the negative number solution is excluded, and the velocity is selectedThe range of degrees is [ v ]6，v7]The solution in (1) is taken as the speed corresponding to the displacement s in the current deceleration and acceleration stage.
6. 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 on 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, the speed is properly reduced when the included angle is an obtuse angle, smooth transition is realized, and the speed is rapidly reduced 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 true CN113635301A (en) 2021-11-12
CN113635301B 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)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114310921A (en) * 2022-03-16 2022-04-12 珞石（北京）科技有限公司 Linear transition path generation method with minimum curvature
WO2023134746A1 (en) * 2022-01-13 2023-07-20 达闼机器人股份有限公司 Speed control method and apparatus for movable device, and movable device, storage medium and computer program
CN116572257A (en) * 2023-07-13 2023-08-11 湖南尖山智能科技有限责任公司 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

Cited By (5)

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

Also Published As

Publication number Publication date
CN113635301B (en) 2023-02-28

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
CN107263484B (en) Robot joint space point-to-point motion trajectory planning method
CN112757306B (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN108241339B (en) Motion solving and configuration control method of humanoid mechanical arm
CN103080859B (en) Trajectory control device
Zhang et al. Curve fitting and optimal interpolation on CNC machines based on quadratic B-splines
CN105159306A (en) Four-rotor aircraft sliding-mode control method based on global stability
CN111158318B (en) Flexible acceleration and deceleration planning method for asymmetric quartic curve
CN114952868B (en) 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot
Zhang et al. Time-optimal trajectory planning of serial manipulator based on adaptive cuckoo search algorithm
CN110515348B (en) Servo motor model selection method of machine tool
CN115922687B (en) Motion trail planning method and device based on S-shaped speed curve
CN112597612B (en) Robot optimization method, device, terminal equipment and computer readable storage medium
CN108472808B (en) Synchronization of multiple robots
CN108829032B (en) Interpolation method for high-precision control
CN107678276A (en) A kind of adaptive composite control method based on turning table control
CN111547177B (en) Self-balancing robot control method and system, self-balancing robot and medium
CN116277036B (en) Rapid fault-tolerant vibration suppression control method for flexible-base and flexible-arm space robot
CN115122329B (en) Convenient industrial robot joint space trajectory planning method
CN115202293B (en) Two-section type speed planning method for industrial robot
CN110501899A (en) A kind of PID follow-up control method based on genetic algorithm parameter Self-tuning System
CN114454180B (en) Motion control method and device of mobile robot and mobile robot
CN116088498A (en) Automatic driving vehicle track tracking control method based on MPC and FPGA
CN109002575A (en) A kind of composite rotor craft transition flight scheme control distribution 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