CN116262346A - Robot motion planning method and device, robot and storage medium - Google Patents
Robot motion planning method and device, robot and storage medium Download PDFInfo
- Publication number
- CN116262346A CN116262346A CN202111521232.8A CN202111521232A CN116262346A CN 116262346 A CN116262346 A CN 116262346A CN 202111521232 A CN202111521232 A CN 202111521232A CN 116262346 A CN116262346 A CN 116262346A
- Authority
- CN
- China
- Prior art keywords
- generalized
- acceleration
- path
- robot
- path point
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000001133 acceleration Effects 0.000 claims abstract description 130
- 230000006870 function Effects 0.000 claims description 51
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000036461 convulsion Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000000758 substrate Substances 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- 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
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Abstract
The invention relates to a robot motion planning method, a robot motion planning device, a robot and a storage medium, wherein the method comprises the following steps: acquiring the generalized arc length of a path point of the robot joint and a function of the path on the generalized arc length; acquiring a generalized speed section of each path point of the robot joint according to the generalized arc length of the path point and a function of the path on the generalized arc length, and acquiring generalized speed and generalized acceleration of the path point according to the generalized speed section; judging whether points with discontinuous acceleration exist in path points of the robot joint according to the generalized acceleration, and if so, performing generalized velocity interpolation near the discontinuous acceleration points; and determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation. The method provided by the invention realizes the motion planning of the robot with optimal time, and ensures that the joint acceleration of the robot is continuous and bounded.
Description
Technical Field
The present invention relates to the field of robot motion control technologies, and in particular, to a robot motion planning method, a device, a robot, and a storage medium.
Background
Motion planning is one of the most important links of industrial robots, and is almost impossible to leave about any application of industrial robots. The motion planning of the robot has strong nonlinearity, particularly, the motion meeting the motion requirement in the Cartesian space is difficult to realize in the joint space near the singular point of the robot, so that the motion planning algorithm on the machine tool cannot be directly applied to the industrial robot; in the prior art, a time-optimal robot motion planning method is proposed for the problem, but the algorithm cannot ensure continuous and bounded acceleration, and has no use value in the actual production process.
Disclosure of Invention
Based on the above, the invention provides a robot motion planning method, which comprises the following steps:
acquiring the generalized arc length of a path point of the robot joint and a function of the path on the generalized arc length;
acquiring a generalized speed interval of each path point of the robot joint according to the generalized arc length of the path point and a function of the path on the generalized arc length, and acquiring generalized speed and generalized acceleration of the path point according to the generalized speed interval;
judging whether points with discontinuous acceleration exist in the path points of the robot joint according to the generalized acceleration, and if so, performing generalized velocity interpolation near the discontinuous acceleration points;
and determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation.
Optionally, obtaining a generalized velocity interval at each of the path points of the robot joint according to the generalized arc length of the path points and a function of the path with respect to the generalized arc length, including:
determining constraint conditions according to the generalized arc length of each path point of the robot joint and the function of the path about the generalized arc length;
and performing linear programming by taking the generalized speed and the generalized acceleration as variables, acquiring the maximum generalized speed and the minimum generalized speed allowed by the current path point, and taking a section formed by the minimum generalized speed and the maximum generalized speed as a generalized speed section of the current path point.
Optionally, when the maximum generalized speed allowed by the current path point is acquired, the corresponding linear programming is described as
max x
min u i+1 ≤u≤u max
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max The maximum linear velocity of the tail end of the robot in the world coordinate system is represented by J, which is the serial number of the robot shaft, and J is a Jacobian matrix.
Optionally, when the minimum generalized speed allowed by the current path point is obtained, the corresponding linear programming is described as
min x
min u i+1 ≤u≤u max
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max The maximum linear velocity of the tail end of the robot in the world coordinate system is represented by J, which is the serial number of the robot shaft, and J is a Jacobian matrix.
Optionally, when determining the lower acceleration limit of the path point, if the maximum generalized speed allowed by the current path point is greater than the maximum generalized speed corresponding to the previous path point, updating the lower acceleration limit of the next path point according to the maximum generalized speed allowed by the current path point.
Optionally, acquiring the generalized velocity and generalized acceleration of the path point according to the generalized velocity interval includes:
and determining constraint conditions according to the generalized speed interval, and performing linear programming by taking generalized speed and generalized acceleration as variables to obtain the generalized speed and generalized acceleration of each path point.
Alternatively, the linear programming is described as
max u
min u i ≤u≤ max u i
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, and p' (l) and p "(l) are the functions of the path on the generalized arc length respectivelyA first derivative and a second derivative, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max Maximum linear velocity of the robot tip in world coordinate system, +.>
For the generalized speed at the current waypoint, max u i and J is the sequence number of the robot axis and J is the Jacobian matrix for the generalized acceleration upper limit under the current path point.
Optionally, according to the generalized acceleration, judging whether a point with discontinuous acceleration exists in the robot joint path points includes: and if the absolute value of the generalized acceleration difference value between the current path point and the previous path point is larger than a preset threshold value, judging that the current path point is a point with discontinuous acceleration.
Optionally, determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized velocity after interpolation, including:
acquiring a time sequence corresponding to the robot joint according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation;
and fitting the generalized arc length of the path point of the robot joint and the time sequence to obtain a function of the path on time.
The invention also provides a robot motion planning device, which comprises a data processing module, a linear planning module, an interpolation module and a path planning module;
the data processing module is used for acquiring the generalized arc length of the path point of the robot joint and a function of the path on the generalized arc length;
the linear programming module is used for acquiring a generalized speed interval of each path point of the robot joint according to the generalized arc length of the path point and a function of the path on the generalized arc length, and acquiring generalized speed and generalized acceleration of the path point according to the generalized speed interval;
the interpolation module is used for judging whether points with discontinuous acceleration exist in the path points of the robot joint according to the generalized acceleration, and if so, performing generalized speed interpolation near the discontinuous acceleration points;
the path planning module is used for determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation.
The invention also provides a robot comprising a processor and a memory coupled with the processor, wherein the memory stores program instructions executable by the processor; and when executing the program instructions stored in the memory, the processor realizes the robot motion planning method in any technical scheme.
The invention also provides a storage medium, wherein the storage medium stores program instructions, and the program instructions realize the robot motion planning method in any technical scheme when being executed by a processor.
According to the robot motion planning method, the robot motion planning device, the robot and the storage medium, the generalized arc length of the robot joint path point and the function of the path about the generalized arc length are obtained, so that the generalized speed interval at the robot joint path point is obtained, the generalized speed and the generalized acceleration at the robot joint path point are obtained according to the generalized speed interval, generalized speed interpolation is carried out near the discontinuous acceleration point, and the function of the path about time is determined according to the generalized arc length of the robot joint path point and the generalized speed after interpolation; the motion planning of the robot with optimal time is realized, the continuous and bounded acceleration of the robot joint is ensured, and the motion track of the robot joint is smooth.
Drawings
Fig. 1 is a schematic flow chart of a robot motion planning method according to an embodiment of the present invention;
FIG. 2 is a two-dimensional plot of paths versus generalized arc length provided by an embodiment of the present invention;
FIG. 3 is a two-dimensional plot of first order derivatives at a path point column versus generalized arc length provided by an embodiment of the present invention;
FIG. 4 is a two-dimensional plot of second derivative at a path point column versus generalized arc length provided by an embodiment of the present invention;
FIG. 5 is a two-dimensional plot of allowed speed intervals over a generalized arc length on a waypoint line provided by an embodiment of the present invention;
FIG. 6 is a two-dimensional plot of the square of generalized acceleration and generalized velocity over generalized arc length for a waypoint provided by an embodiment of the present invention;
FIG. 7 is a graph of generalized velocity versus generalized arc length provided by an embodiment of the present invention;
FIG. 8 is a two-dimensional plot of time series versus generalized arc length provided by an embodiment of the present invention;
FIG. 9 is a two-dimensional plot of path versus time provided by an embodiment of the present invention;
fig. 10 is a schematic structural diagram of a robot motion planning device according to an embodiment of the present invention;
fig. 11 is a schematic structural diagram of a storage medium according to an embodiment of the present invention.
Detailed Description
In order that the invention may be readily understood, a more complete description of the invention will be rendered by reference to the appended drawings. Preferred embodiments of the present invention are shown in the drawings. This invention may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.
It will be understood that when an element is referred to as being "fixed to" another element, it can be directly on the other element or intervening elements may also be present. When an element is referred to as being "connected" to another element, it can be directly connected to the other element or intervening elements may also be present.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention.
Fig. 1 is a flow chart of a robot motion planning method according to a first embodiment of the present invention. It should be noted that, if there are substantially the same results, the method of the present invention is not limited to the flow sequence shown in fig. 1.
As shown in fig. 1, the robot motion planning method mainly includes the following steps S101 to S104:
s101, acquiring a generalized arc length of a path point of a robot joint and a function of the path about the generalized arc length;
wherein, the robot joint path points form a robot joint path point row, the arc length corresponding to the path points can be obtained according to the robot joint path point row, and the calculation formula of the arc length corresponding to the path points is l i+1 ＝l i +|p i+1 -p i | 2 ，l i Is the path point p i Corresponding arc length l i+1 Is the path point p i+1 The corresponding arc length is obtained by calculating the arc length corresponding to the path point row by using the chord length to replace the arc length approximately because the path points are closely spaced, and then the function p (l) of the path relative to the generalized arc length can be obtained by using cubic spline curve fitting.
It should be noted that the motion planning according to the embodiments of the present invention is a planning in joint space (joint coordinate system), and may be a function of a generalized arc length for a robot having six joints, where the robot has six dimensions of paths.
In one embodiment, the robot kinematics parameters are shown in table 1,
table 1 robot kinematics parameters
In table 1, link number is a joint number, a is a distance between two joint axes along a coaxial line, α is an included angle between the two joint axes, d is a joint translation distance, and θ is a joint translation angle. The maximum speed of each joint of the robot is pi/s, and the maximum acceleration of each joint is 10 pi/s 2 The path point sequence is subjected to cubic spline fitting, and a two-dimensional graph of the path relative to the generalized arc length is obtained, as shown in fig. 2. After obtaining the function of the path point row about the generalized arc length, obtaining the first-order derivative and the second-order derivative of the function at the path point row, and respectively obtaining two-dimensional graphs of the first-order derivative and the second-order derivative at the path point row about the generalized arc length as shown in fig. 3 and 4, and q in fig. 3 and 4 1 -q 6 Six different joints are shown.
S102, acquiring a generalized speed section of each path point of the robot joint according to the generalized arc length of the path point and a function of the path on the generalized arc length, and acquiring generalized speed and generalized acceleration of the path point according to the generalized speed section;
it should be noted that, for each joint of the robot, the generalized speed cannot exceed the generalized speed interval, and the inverse iterative calculation is performed according to the constraint condition of the function of the path about the generalized arc length, that is, from back to front in time sequence, the allowable generalized speed interval at the path point sequence can be obtained.
S103, judging whether points with discontinuous acceleration exist in the path points of the robot joint according to the generalized acceleration, and if so, performing generalized velocity interpolation near the discontinuous acceleration points;
if there is no point where acceleration is discontinuous, interpolation is not necessary, and interpolation of the generalized velocity in the vicinity of the point where acceleration is discontinuous is performed, that is, a plurality of points (for example, 20 points) are inserted around the point where acceleration is discontinuous, and each point is separated from the other by a predetermined arc length (for example, 0.02 rad).
S104, determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation.
According to the robot motion planning method provided by the embodiment of the invention, the generalized arc length of the robot joint path point and the function of the path about the generalized arc length are obtained, so that the generalized speed interval at the robot joint path point is obtained, the generalized speed and the generalized acceleration at the robot joint path point are obtained according to the generalized speed interval, generalized speed interpolation is carried out near the discontinuous acceleration point, and the function of the path about time is determined according to the generalized arc length of the robot joint path point and the generalized speed after interpolation; the motion planning of the robot with optimal time is realized, the continuous and bounded acceleration of the robot joint is ensured, the robot joint can smoothly pass through the singular points of the robot, and the motion track of the robot joint is smooth.
In an alternative embodiment, obtaining a generalized velocity interval at each of the path points of the robotic joint from the generalized arc length of the path points and a function of the path with respect to the generalized arc length includes:
determining constraint conditions according to the generalized arc length of each path point of the robot joint and the function of the path about the generalized arc length;
and performing linear programming by taking the generalized speed and the generalized acceleration as variables, acquiring the maximum generalized speed and the minimum generalized speed allowed by the current path point, and taking a section formed by the minimum generalized speed and the maximum generalized speed as a generalized speed section of the current path point.
According to constraint conditions of functions of the path on generalized arc lengths, reverse iterative computation is conducted, the maximum generalized speed and the minimum generalized speed at the path point row can be obtained, and therefore the allowed generalized speed interval at the path point row can be determined.
In an alternative embodiment, the corresponding linear programming is described as when the maximum generalized velocity allowed by the current waypoint is obtained
max x
min u i+1 ≤u≤u max
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max The maximum linear velocity of the tail end of the robot in the world coordinate system is represented by J, which is the serial number of the robot shaft, and J is a Jacobian matrix.
The linear programming is a second-order linear programming, and variables are x and u respectively; the lower limit of the generalized acceleration is obtained by reverse iteration, so that the lower limit is changed along with the reverse iteration process, and the upper limit of the generalized acceleration u max The third row constraint in the constraint condition represents the speed constraint of each axis of the robot, the fourth row constraint in the constraint condition is the acceleration constraint of each axis (joint) of the robot, and the fifth row constraint in the constraint condition is the linear speed constraint of the tail end of the robot in the world coordinate system; the sixth constraint in the constraint conditions establishes the constraint of the front and rear two path points, namely, uniform acceleration movement is carried out between the two path points; the current path point position can be obtained through the solution of the linear programmingSquare of maximum allowable generalized speed max x i 。
In one embodiment, the 500 th waypoint is taken as an example, and the waypoint is q500= [ -0.0000-0.2640 0.2358 0.0000 0.0283-0.0000]The first order derivative at the path point with respect to the generalized arc length is q500' = [ 0.00000.7584-0.6298 0.0000-0.1286-0.0000]The second derivative is q500 "= [ -0.0000-1.1484-1.7885-0.0000 2.9369 0.0000]， min x 501 Is set to be 0, the number of the components is set to be 0,
let 10, u max Set to 50, v max The temperature was set to be 0.25, max x 501 in the form of 1.3871, min u 501 is-0.06; thus solve for max x 500 The linear programming problem of (2) can be expressed as
max x
s.t.0≤x≤100
-0.06≤u≤50
0<0x<π 2
0<0.5751x<π 2
0<0.3966x<π 2
0<0x<π 2
0<0.0165x<π 2
0<0x<π 2
-10π<0x+0u<10π
-10π<-1.1484x+0.7584u<10π
-10π<-1.7885x-0.6298u<10π
-10π<0x+0u<10π
-10π<2.9369x-0.1286u<10π
-10π<0x+0u<10π
0≤0.0322x≤0.0625
0≤x+0.004u≤1.3871
Solving out max x 500 1.3752.
In an alternative embodiment, the corresponding linear programming is described as when the minimum generalized velocity allowed by the current waypoint is obtained
min x
min u i+1 ≤u≤u max
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max The maximum linear velocity of the tail end of the robot in the world coordinate system is represented by J, which is the serial number of the robot shaft, and J is a Jacobian matrix.
It should be noted that the above linear programming is also a second order linear programming, the variables are also x and u, respectively, and the square of the minimum generalized speed allowed by the current path point can be obtained by solving the above linear programming problem min x i 。
Tool withIn an embodiment of the body of the present invention, min x 500 can be obtained by solving the following linear problem
min x
s.t.0≤x≤100
-0.06≤u≤50
0<0x<π 2
0<0.5751x<π 2
0<0.3966x<π 2
0<0x<π 2
0<0.0165x<π 2
0<0x<π 2
-10π<0x+0u<10π
-10π<-1.1484x+0.7584u<10π
-10π<-1.7885x-0.6298u<10π
-10π<0x+0u<10π
-10π<2.9369x-0.1286u<10π
-10π<0x+0u<10π
0≤0.0322x≤0.0625
0≤x+0.004u≤1.3871
Solving out min x 500 0 due to max x 500 < max x 501 (acceleration section), therefore min u 500 And min u 501 take the same value, i.e min u 501 -0.06, a two-dimensional plot of the allowed speed interval on the path point versus the generalized arc length, as shown in fig. 5.
In an optional embodiment, the robot motion planning method further includes, when determining the lower acceleration limit of the path point, if the maximum generalized speed allowed by the current path point is greater than the maximum generalized speed corresponding to the previous path point, updating the lower acceleration limit of the next path point according to the maximum generalized speed allowed by the current path point.
When determining the generalized acceleration lower limit, if the maximum generalized speed allowed by the current step (the current path point) is larger than the maximum generalized speed allowed by the previous iteration (the previous path point), updating the generalized acceleration lower limit of the next iteration (the next path point) according to the maximum generalized speed allowed by the current step, otherwise, the generalized acceleration lower limit of the next iteration is consistent with the generalized acceleration of the current step.
When the maximum generalized speed allowed by the previous step is larger than the maximum generalized speed allowed by the previous iteration, the calculation formula of the acceleration lower limit of the next iteration is as follows min u i ＝ min u i+1 –k·(l i+1 -l i )/√( max x i ) In the formula, k is a preset generalized jump upper limit, and this step is actually equivalent to adding a constraint in the second-order linear programming to ensure that the generalized acceleration of the deceleration section is continuous and the generalized jerk is bounded.
In an alternative embodiment, obtaining the generalized velocity and generalized acceleration of the waypoint according to the generalized velocity interval includes:
and determining constraint conditions according to the generalized speed interval, and performing linear programming by taking generalized speed and generalized acceleration as variables to obtain the generalized speed and generalized acceleration of each path point.
It should be noted that, linear programming is performed by taking the generalized speed and the generalized acceleration as variables, so as to obtain the generalized speeds and the generalized accelerations of all the path points, and the iteration process is forward iteration, i.e. from front to back in time sequence.
In an alternative embodiment, the linear programming is described as
max u
min u i ≤u≤ max u i
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max Maximum linear velocity of the robot tip in world coordinate system, +.>
For the generalized speed at the current waypoint, max u i and J is the sequence number of the robot axis and J is the Jacobian matrix for the generalized acceleration upper limit under the current path point.
It should be noted that the number of the substrates,
for the generalized speed at the current waypoint, max u i is the upper limit that varies with the iterative process; solving the linear programming problem to obtain generalized acceleration u of the current path point i Generalized speed under next path point +.>
The method can be calculated by the following formula:
the upper acceleration limit of the next path point can be calculated by the following formula:
this step is actually equivalent to adding a constraint to the above-mentioned linear rule, so that the generalized acceleration of the acceleration section is continuous and bounded.
In one embodiment, taking the 500 th waypoint as an example, there is the following linear programming problem:
maxx
s.t.1.3191≤x≤1.3191
-0.06≤u≤2.2957
0<0x<π 2
0<0.5751x<π 2
0<0.3966x<π 2
0<0x<π 2
0<0.0165x<π 2
0<0x<π 2
-10π<0x+0u<10π
-10π<-1.1484x+0.7584u<10π
-10π<-1.7885x-0.6298u<10π
-10π<0x+0u<10π
-10π<2.9369x-0.1286u<10π
-10π<0x+0u<10π
0≤0.0322x≤0.0625
0≤x+0.004u≤1.3871
solving for u 500 2.2838, thus x of the next iteration 501 =1.3191+2×0.002×2.2838= 1.3282; the generalized upper acceleration limit for the next iteration can be calculated by
max u 501 ＝2.2838+50*0.002/√1.3282＝2.3706
A two-dimensional plot of the square of the generalized acceleration and generalized velocity over the generalized arc length at the path point is shown in fig. 6.
In an optional embodiment, determining whether there is a point with discontinuous acceleration in the robot joint path points according to the generalized acceleration includes: and if the absolute value of the generalized acceleration difference value between the current path point and the previous path point is larger than a preset threshold value, judging that the current path point is a point with discontinuous acceleration.
Wherein, can be inequality |u i+1 -u i |>Δu max Judging whether points with discontinuous acceleration exist in the robot joint path points, if the inequality is met, judging that the points with discontinuous acceleration exist in the robot joint path points, and interpolating the points near the (i+1) th path point, specifically, interpolating the generalized speed near the discontinuous acceleration points by using a cubic Bezier curve to ensure that the generalized acceleration is continuous and the acceleration is bounded.
In one embodiment, the points of discontinuity in acceleration are determined based on the calculated acceleration, Δu is set to 3, and it can be seen from fig. 6 that there are four points of discontinuity in acceleration, respectively near the generalized arc lengths 0.06,0.9,1.8 and 2.5, and the corresponding path point columns are numbered 88, 528, 968, 1308, respectively. Interpolation is carried out on the generalized speed near the discontinuous points of the acceleration by using a cubic Bezier curve, so that the continuity of the generalized acceleration and the limitation of the jerk can be ensured; as shown in fig. 7, the solid line in fig. 7 is a curve of the generalized velocity with respect to the generalized arc length before interpolation, and the broken line is a curve of the generalized velocity with respect to the generalized arc length after interpolation.
In an alternative embodiment, determining a function of the path of the robot joint with respect to time based on the generalized arc length of each of the path points of the robot joint and the generalized velocity after interpolation, includes:
acquiring a time sequence corresponding to the robot joint according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation;
and fitting the generalized arc length of the path point of the robot joint and the time sequence to obtain a function of the path on time.
The specific calculation formula is that the time sequence corresponding to the robot joint path point is obtained according to the generalized arc length of the robot joint path point and the generalized speed after interpolation
And fitting the generalized arc length of the robot joint path point and the time sequence by using a cubic spline curve to obtain a function of the path with respect to time.
In one embodiment, a two-dimensional plot of time series versus generalized arc length is obtained by fitting a cubic spline curve to a path versus time function p (t), as shown in FIG. 8, and a two-dimensional plot of path versus time is obtained, as shown in FIG. 9, q in FIG. 9 1 -q 6 Six different joints are shown.
As shown in fig. 10, a robot motion planning device 100 according to a second embodiment of the present invention includes a data processing module 101, a linear planning module 102, an interpolation module 103, and a path planning module 104; the data processing module 101 is configured to obtain a generalized arc length of a path point of the robot joint and a function of the path with respect to the generalized arc length; the linear programming module 102 is configured to obtain a generalized speed interval of each path point of the robot joint according to a generalized arc length of the path point and a function of the path about the generalized arc length, and obtain a generalized speed and a generalized acceleration of the path point according to the generalized speed interval; the interpolation module 103 is configured to determine, according to the generalized acceleration, whether a point with discontinuous acceleration exists in the path points of the robot joint, and if so, perform generalized velocity interpolation near the discontinuous acceleration point; the path planning module 104 is configured to determine a function of a path of the robot joint with respect to time according to a generalized arc length of each path point of the robot joint and a generalized velocity after interpolation.
A third embodiment of the present invention provides a robot including a processor, and a memory coupled to the processor, the memory storing program instructions executable by the processor; the robot motion planning method according to any one of the above embodiments is implemented when the processor executes the program instructions stored in the memory.
Fig. 11 is a schematic structural diagram of a storage medium according to a fourth embodiment of the present invention, where the storage medium 110 stores program instructions 111, and the program instructions 111 implement the robot motion planning method according to any of the above embodiments when executed by a processor. The storage medium may be nonvolatile or volatile. Wherein the program instructions 111 may be stored in the storage medium as a software product, and the storage medium comprises: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
The technical features of the above embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The foregoing examples only represent preferred embodiments of the present invention, which are described in more detail and are not to be construed as limiting the scope of the invention. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the invention, which are all within the scope of the invention. Accordingly, the scope of protection of the present invention is to be determined by the appended claims.
Claims (12)
1. The robot motion planning method is characterized by comprising the following steps:
acquiring the generalized arc length of a path point of the robot joint and a function of the path on the generalized arc length;
acquiring a generalized speed interval of each path point of the robot joint according to the generalized arc length of the path point and a function of the path on the generalized arc length, and acquiring generalized speed and generalized acceleration of the path point according to the generalized speed interval;
judging whether points with discontinuous acceleration exist in the path points of the robot joint according to the generalized acceleration, and if so, performing generalized velocity interpolation near the discontinuous acceleration points;
and determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation.
2. The robot motion planning method of claim 1, wherein obtaining a generalized velocity interval at each of the path points of the robot joint from the generalized arc length of the path points and a function of the path with respect to the generalized arc length, comprises:
determining constraint conditions according to the generalized arc length of each path point of the robot joint and the function of the path about the generalized arc length;
and performing linear programming by taking the generalized speed and the generalized acceleration as variables, acquiring the maximum generalized speed and the minimum generalized speed allowed by the current path point, and taking a section formed by the minimum generalized speed and the maximum generalized speed as a generalized speed section of the current path point.
3. The robot motion planning method according to claim 2, wherein the corresponding linear programming is described as when the maximum generalized velocity allowed by the current waypoint is obtained
max x
min u i+1 ≤u≤u max
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max The maximum linear velocity of the tail end of the robot in the world coordinate system is represented by J, which is the serial number of the robot shaft, and J is a Jacobian matrix.
4. The robot motion planning method according to claim 2, wherein the corresponding linear plan is described as when the minimum generalized velocity allowed by the current waypoint is obtained
min x
min u i+1 ≤u≤u max
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max An upper limit of the generalized acceleration u is indicated, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max The maximum linear velocity of the tail end of the robot in the world coordinate system is represented by J, which is the serial number of the robot shaft, and J is a Jacobian matrix.
5. The method according to claim 3 or 4, further comprising, when determining the lower acceleration limit of a path point, if the maximum generalized speed allowed by the current path point is greater than the maximum generalized speed corresponding to the previous path point, updating the lower acceleration limit of the next path point according to the maximum generalized speed allowed by the current path point.
6. The robot motion planning method according to claim 1, wherein acquiring the generalized velocity and generalized acceleration of the waypoint from the generalized velocity interval comprises:
and determining constraint conditions according to the generalized speed interval, and performing linear programming by taking generalized speed and generalized acceleration as variables to obtain the generalized speed and generalized acceleration of each path point.
7. The robotic motion planning method of claim 6, wherein the linear programming is described as
max u
min u i ≤u≤ max u i
0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2
min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1
Where x is the square of the generalized velocity, u is the generalized acceleration,
as the upper limit of the generalized speed, u max Representing a broad senseThe upper limit of the acceleration u is set, min u i for the generalized acceleration lower limit under the current path point, the subscript i is the sequence number of the current path point, p' (l), p "(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 square of maximum generalized velocity allowed for i+1th waypoint, v max Maximum linear velocity of the robot tip in world coordinate system, +.>
For the generalized speed at the current waypoint, max u i and J is the sequence number of the robot axis and J is the Jacobian matrix for the generalized acceleration upper limit under the current path point.
8. The robot motion planning method according to claim 1, wherein determining whether there is a point of discontinuous acceleration among the robot joint path points according to the generalized acceleration includes: and if the absolute value of the generalized acceleration difference value between the current path point and the previous path point is larger than a preset threshold value, judging that the current path point is a point with discontinuous acceleration.
9. The robot motion planning method of claim 1, wherein determining a function of a path of the robot joint with respect to time based on a generalized arc length of each of the path points of the robot joint and a generalized velocity after interpolation, comprises:
acquiring a time sequence corresponding to the robot joint according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation;
and fitting the generalized arc length of the path point of the robot joint and the time sequence to obtain a function of the path on time.
10. The robot motion planning device is characterized by comprising a data processing module, a linear programming module, an interpolation module and a path planning module;
the data processing module is used for acquiring the generalized arc length of the path point of the robot joint and a function of the path on the generalized arc length;
the linear programming module is used for acquiring a generalized speed interval of each path point of the robot joint according to the generalized arc length of the path point and a function of the path on the generalized arc length, and acquiring generalized speed and generalized acceleration of the path point according to the generalized speed interval;
the interpolation module is used for judging whether points with discontinuous acceleration exist in the path points of the robot joint according to the generalized acceleration, and if so, performing generalized speed interpolation near the discontinuous acceleration points;
the path planning module is used for determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation.
11. A robot comprising a processor, and a memory coupled to the processor, the memory storing program instructions executable by the processor; the robot motion planning method according to any one of claims 1-9 when executed by the processor.
12. A storage medium having stored therein program instructions which when executed by a processor implement the robot motion planning method of any of claims 1-9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111521232.8A CN116262346A (en) | 2021-12-13 | 2021-12-13 | Robot motion planning method and device, robot and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111521232.8A CN116262346A (en) | 2021-12-13 | 2021-12-13 | Robot motion planning method and device, robot and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116262346A true CN116262346A (en) | 2023-06-16 |
Family
ID=86723214
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111521232.8A Pending CN116262346A (en) | 2021-12-13 | 2021-12-13 | Robot motion planning method and device, robot and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116262346A (en) |
-
2021
- 2021-12-13 CN CN202111521232.8A patent/CN116262346A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105500354B (en) | Transitional track planning method applied by industrial robot | |
| EP2022608B1 (en) | Improved blending algorithm for trajectory planning | |
| CN106863306B (en) | Robot joint space smooth track planning method | |
| EP1696294A2 (en) | Method and apparatus for generating teaching data for a robot | |
| US9764462B2 (en) | Robot apparatus and robot controlling method | |
| CN112757306A (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
| Wang et al. | Optimal trajectory planning of grinding robot based on improved whale optimization algorithm | |
| CN112222703B (en) | Energy consumption optimal trajectory planning method for welding robot | |
| TWI704039B (en) | Mechanical arm singular point control method and system | |
| Shen et al. | A smooth and undistorted toolpath interpolation method for 5-DoF parallel kinematic machines | |
| CN111633668A (en) | Motion control method for robot to process three-dimensional free-form surface | |
| JP2007000954A (en) | Robot teaching device and method | |
| CN113263496B (en) | Method for optimizing path of six-degree-of-freedom mechanical arm and computer equipment | |
| JPH04111006A (en) | Path interpolating method for robot | |
| CN113084792A (en) | Method for determining joint singular area, robot and storage device | |
| CN113103240B (en) | Method, device and system for realizing C2 continuous robot trajectory planning | |
| US20210323154A1 (en) | Disassembly based assembly planning | |
| CN116262346A (en) | Robot motion planning method and device, robot and storage medium | |
| Furukawa | Time-subminimal trajectory planning for discrete non-linear systems | |
| Riboli et al. | Collision-free and smooth motion planning of dual-arm Cartesian robot based on B-spline representation | |
| Tangpattanakul et al. | Optimal trajectory of robot manipulator using harmony search algorithms | |
| CN111496798B (en) | Robot conveyor belt tracking method, equipment and storage device | |
| CN117301063A (en) | Industrial robot online look-ahead track planning method, electronic equipment and storage medium | |
| CN113650011B (en) | Method and device for planning splicing path of mechanical arm | |
| CN113442142B (en) | Smoothing method for Cartesian space motion trail of six-axis serial industrial robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |