CN116079714A - Six-axis mechanical arm track planning method based on B spline - Google Patents
Six-axis mechanical arm track planning method based on B spline Download PDFInfo
- Publication number
- CN116079714A CN116079714A CN202211539346.XA CN202211539346A CN116079714A CN 116079714 A CN116079714 A CN 116079714A CN 202211539346 A CN202211539346 A CN 202211539346A CN 116079714 A CN116079714 A CN 116079714A
- Authority
- CN
- China
- Prior art keywords
- interpolation
- spline
- speed
- parameter
- mechanical arm
- 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 41
- 230000036461 convulsion Effects 0.000 claims abstract description 19
- 230000001133 acceleration Effects 0.000 claims abstract description 16
- 238000005070 sampling Methods 0.000 claims abstract description 15
- 230000010354 integration Effects 0.000 claims abstract description 8
- 238000012805 post-processing Methods 0.000 claims abstract description 8
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 5
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 239000013589 supplement Substances 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 4
- 238000005498 polishing Methods 0.000 description 4
- 238000005507 spraying Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000002349 favourable effect Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000006872 improvement Effects 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
-
- 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]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a six-axis mechanical arm track planning method based on B spline, which comprises the steps of firstly, customizing control point parameters and fitting modes, and fitting out corresponding B spline equations according to the control point parameters; the method comprises the steps of performing Simpson numerical integration on a B spline parameter equation to obtain the length of each piecewise function curve and the total length of the B spline curve, sampling the curvature of the B spline parameter equation, calculating the minimum running time according to the sampling speed, dividing the interpolation time of a minimum integer to obtain the maximum limiting speed of each piece by the minimum interpolation period, and finally obtaining a smooth speed plan according to the convolution of jerk and acceleration limitation twice, calculating interpolation parameters and interpolation points, and finishing planned track interpolation; finally, post-processing is carried out on the planned interpolation points, the points are taken from the interpolation points in a concentrated mode, an actuator is issued, the replacement adaptability is good conveniently through a modularized process, and the tail end of the mechanical arm moves smoothly and has small impact through overall linear velocity look-ahead planning.
Description
Technical Field
The invention relates to the technical field of robot track planning, in particular to a six-axis mechanical arm track planning method based on B-spline.
Background
In the motion control of the mechanical arm, common planning modes are divided into joint motion planning and end pose planning, wherein the end pose motion planning commonly comprises linear motion, circular motion and spline track motion. The method is usually implemented by spraying, polishing, massaging, assembling and the like on a mechanical arm aiming at tracking complex curves and curved surfaces, and a currently preferred method is to use a mode of constructing B-spline. The B spline has the advantages of parameterization, geometric invariance, local propping property and the like, is widely applied to the industrial field, and has gentle curvature change of the higher-order B spline and small impact on a system. However, the current spline curve planning has the problems that the global speed control is difficult to meet the manufacturability, the interpolation speed error is large when the long-distance curvature change range is large, the planning engineering is inconvenient to add force and position mixed control, and the like.
Disclosure of Invention
The invention provides a six-axis mechanical arm track planning method based on B-spline, which solves the problems that the global speed control is difficult to meet the manufacturability, the interpolation speed error is large when the long-distance curvature change range is large, the force and position hybrid control is inconvenient to add in planning engineering and the like in the current spline curve planning.
In order to achieve the above purpose, the present invention provides the following technical solutions: a six-axis mechanical arm track planning method based on B-spline comprises the following steps:
s1, firstly fitting a terminal motion track, customizing control point parameters and fitting modes, and fitting a corresponding B spline equation according to the control point parameters;
s2, carrying out Simpson numerical integration on the B spline parameter equation obtained in the previous step to obtain the length of each piecewise function curve and the total length of the B spline curve, and simultaneously sampling the curvature of the B spline parameter equation at a given sampling interval to obtain track motion parameters, thereby providing contents for subsequent speed look-ahead planning and interpolation parameter calculation;
s3, planning a motion speed look-ahead, calculating minimum running time according to a sampling speed, dividing and solving a minimum integer through a minimum interpolation period, reversely solving the maximum limiting speed of each segment through the minimum integer interpolation time, and finally convolving twice according to jerk and acceleration limitation to obtain a smooth speed plan;
s4, planning according to a known B spline curve equation and the speed, and solving interpolation parameters and interpolation points to complete planned track interpolation;
and S5, finally, post-processing the planned interpolation points to obtain Cartesian space interpolation pose, and centrally taking the points from the interpolation points to issue an actuator.
Preferably, the control point parameters include a position, a posture, and a weight.
Preferably, the B-spline parameter equation is:
wherein w is i N+1 weight factors corresponding to the control vertexes, and 1 for the quasi-uniform planning factor; d, d i N+1 control vertices; n (N) i，p (U) is a node vector u= [ U ] 0 ，u 1 ，u 2 ，…，u n+p+1 ]The expression of the ith p-th basis function is as follows:
and finally outputting a curve parameter equation C (u).
Preferably, the B-spline parametric equation result includes a radius of curvature of the position portion and a radius of curvature of the posture portion, wherein the unit of the radius of curvature of the position portion is meters, and the unit of the radius of curvature of the posture portion is radians.
Preferably, the Simpson adaptive numerical integration formula is:
wherein a and b are upper and lower limits of parameters of the Simpson sampling interval, and C (u) is a curve parameter equation.
Preferably, the convolution formula is:
f (tau) in the first convolution is the speed limit obtained by the second partial flow, and the final speed limit is obtained after interpolation interval specification; g (x- τ) is the acceleration signal:
g(x-τ)＝1/(Vmax/Acc/Ts)*ones(1，(Vmax/Acc/Ts))
the above formula Vmax is the maximum speed in a given whole speed plan, acc is the given acceleration, and Ts is the interpolation interval time;
f (tau) in the second convolution is the planning speed obtained after the first convolution;
g (x- τ) is a jerk signal:
g(x-τ)＝1/(Acc/Jerk/Ts)*ones(1，(Acc/Jerk/Ts))
the above equation Acc is given acceleration, jerk is given Jerk, and Ts is interpolation interval.
Preferably, in the step S4, the planned trajectory interpolation includes the following steps:
(1) When the interval of the interpolation period is smaller, a first-order Taylor expansion type parameter is adopted, the parameter is substituted into an equation to calculate an interpolation point, a first-order derivative and a second-order derivative of the interpolation point, then the speed error of the interpolation point is calculated according to a bow Gao Wucha to supplement the speed of the interpolation point to obtain an actual speed, then the planning speed of the interpolation point is differenced from the actual speed, when the speed difference is smaller than a specified error epsilon, the next step is carried out, otherwise, the parameter u carries out compensation iteration through the ratio of the planning speed/the actual speed until all the planning is finally completed;
(2) When the interpolation period interval is longer, replacing the first-order Taylor expansion with the n-order Taylor expansion, wherein n is more than 1, and calculating the subsequent interpolation points by using an Adams implicit method or a Dragon library tower method after n+1 values are calculated;
(3) When the ratio of the speed to the curvature radius is large, adopting a dichotomy iteration method, and sacrificing the iteration times to improve the stability;
(4) Establishing a relation between the speed V planned in the step S3 and a parameter u with respect to a parameter equation C (u) to obtain an interpolation parameter u, further bringing the interpolation parameter u into the parameter equation C (u) to obtain an interpolation pose, and iterating in a first-order Taylor expansion mode;
(5) Interpolation parameter u based on last moment i And its relation to velocity V (u i ) The next interpolation parameter u is obtained i+1 ：
Where u is an interpolation parameter, i is an interpolation count, t is time generally t i+1 -t i I.e. a fixed interpolation period Ts,
the relation of velocity V to curve parameter equation C (u) is introduced:
wherein V (u) i ) For the planning speed at the ith interpolation, divided by the parameter equation C (u) at u i The two-norm change rate at the point i is obtained by determining the i interpolation time with respect to the current planned speed V (u i ) The rate of change of the interpolation parameter u;
(6) The expansion cut-off error is reduced by increasing the Taylor expansion order, and the compensation ER is calculated through the curvature radius r of the current interpolation point and the actual speed v, so that the bow height error is reduced by compensating the actual speed;
(7) Calculate C (u) i ) Curvature at
Radius of curvature->
Bow height error->
Corrected actual speed after adding error compensation>
(8) When the interpolation interval time Ts is smaller, the interpolation parameter u is made i Directly correct as
Wherein V (u) i ) For the current plugAnd (3) calculating the point planning speed, and finally substituting the planned parameter u into a parameter equation C (u) to obtain the planned interpolation pose.
8. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: in the step S5, the post-processing flow includes that after a set of cartesian space pose interpolation points of a section of B spline planning is obtained, the point issuing executor is started to be taken from the set of interpolation points, whether the interpolation points enter a force control adjustment module is selected, the interpolation points are modified for the second time, then the cartesian pose of the interpolation points is inversely solved into joint angles theta according to the DH parameters of the robot, and finally the joint angles theta are issued to each joint for execution.
Compared with the prior art, the invention has the beneficial effects that:
1. according to the invention, through global linear velocity look-ahead planning, the tail end of the mechanical arm moves smoothly and has small impact, and the process requirements such as spraying, polishing and the like are met.
2. In the invention, the modularized flow is convenient to replace and has good adaptability, and different flow modules are replaced according to different computing and executing environments and requirements, such as fitting according to on-line or off-line programming selection of different splines.
3. The method can plan a section of track at one time, and then carry out post-processing through a force control algorithm, so that the real-time performance of force control is greatly improved, and the control period is greatly shortened; meanwhile, under the cyclic reciprocating working condition, such as back and forth polishing and massaging, only one calculation is needed, and the calculation load can be repeatedly taken and reduced.
Drawings
The accompanying drawings are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate the invention and together with the embodiments of the invention, serve to explain the invention.
In the drawings:
FIG. 1 is an overall flow chart of the present invention;
FIG. 2 is a speed plan graph of the present invention;
fig. 3 is a diagram of NURBS curve bow height error compensation of the present invention.
Detailed Description
The preferred embodiments of the present invention will be described below with reference to the accompanying drawings, it being understood that the preferred embodiments described herein are for illustration and explanation of the present invention only, and are not intended to limit the present invention.
Examples: as shown in fig. 1, a six-axis mechanical arm track planning method based on B-spline includes five parts: fitting a tail end motion track, acquiring track motion parameters, planning a motion speed in advance, planning track interpolation, and performing track interpolation post-treatment.
A first part: fitting the motion trail of the tail end, and carrying out self definition on the control points and the fitting modes according to the process of fitting the corresponding B spline equation by the control point parameters. Inputting a control point pose, densifying additional control points, enabling a curve to pass through the control points, and fitting the control points by using a 4-order improved quasi-uniform B spline to obtain a segmented curve parameter equation, wherein the overall flow diagram is shown in FIG. 1; or under the condition that each control point and node vector are known, reversely solving a new control point passing through the original function curve, and solving a parameter equation of the non-uniform B spline curve; the mode of only inputting the control point position is suitable for online programming of the robot, is favorable for small input quantity and convenient operation; the mode of inputting the control points and the node vectors is suitable for offline programming of the robot, is favorable for controlling the fitting to be rich in information, and can describe a desired curve more accurately;
the control point parameters include its position (x y z), pose (rx ry rz), and weight ({ w }).
B spline parameter equation:
wherein w is i N+1 weight factors corresponding to the control vertexes, and 1 for the quasi-uniform planning factor; d, d i N+1 control vertices; n (N) i，p (U) is a node vector u= [ U ] 0 ，u 1 ，u 2 ，…，u n+p+1 ]The expression of the ith p-th basis function is as follows:
and finally outputting a curve parameter equation C (u).
A second part: the spline track parameters are obtained to provide content for the subsequent speed look-ahead planning and interpolation parameter calculation; on one hand, the length of each piecewise function curve and the total length of the B spline curve are obtained by carrying out Simpson numerical integration on the B spline parameter equation obtained in the previous step; on the other hand, the curvature of the B spline parameter equation is sampled through a given sampling interval, the result of the B spline parameter equation comprises the curvature radius of the position part and the curvature radius of the gesture part, the position curvature radius unit is meter, the gesture curvature radius unit is radian, the numerical value of the gesture is compared from the maximum acceleration of the robot motor and the length of the connecting rod serving as experience coefficients to the position curvature radius, the same sampling point takes the minimum curvature radius as a forward-looking speed limiting value, and the line segment length is calculated by adopting a Simpson self-adaptive integration mode, so that smaller calculation resources are consumed, and higher numerical precision is obtained; by adopting the speed limit of the terminal pose, the requirements of terminal linear speed such as spraying, polishing, massaging and the like are met, and meanwhile, the impact on a robot system is reduced.
Simpson adaptive numerical integration formula:
wherein a and b are upper and lower limits of parameters of the Simpson sampling interval, and C (u) is a curve parameter equation.
The curvature speed limit uses the method of isocentric acceleration value, the known formula:
when the centripetal acceleration a is set to be a constant value, the sampling curvature radius r determines the speed limit v, namely the speed limit of the track curve at all positions is obtained, and the value of a is set according to the performance and experience of the mechanical arm.
Third section: track speed look ahead, since the computer system is a discretized system, there is an interpolation interval, and in order to accurately arrive at a bit in a cycle, a normalized speed and time are required. The minimum running time is calculated according to the sampling speed, the minimum integer is calculated through the minimum interpolation period division, and the maximum limiting speed of each segment is calculated through the minimum integer interpolation time. Finally, by means of the convolution property, the track-out speed is rapidly planned with small calculation cost and good adaptability, acceleration and jerk are introduced, and the maximum speed limit of the curve track of the step is divided into two convolutions to form a smooth speed planning curve, as shown in fig. 2;
convolution formula:
f (tau) in the first convolution is the speed limit obtained by the second partial flow, and the final speed limit is obtained after interpolation interval specification; g (x- τ) is the acceleration signal:
g(x-τ)＝1/(Vmax/Acc/Ts)*ones(1，(Vmax/Acc/Ts))
the above formula Vmax is the maximum speed (m/s) in a given overall speed plan, acc is the given acceleration (m/s 2 ) Ts is the interpolation interval time(s).
F (tau) in the second convolution is the planning speed obtained after the first convolution; g (x- τ) is a jerk signal:
g(x-τ)＝1/(Acc/Jerk/Ts)*ones(1，(Acc/Jerk/Ts))
the above Acc is given acceleration (m/s 2 ) Jerk is given Jerk (m/s 3 ) Ts is the interpolation interval time(s).
Fourth part: and (3) track speed interpolation, and solving interpolation parameters and interpolation points according to a known B spline curve equation and a speed plan. According to the relation between the speed and the parameter and the curve equation, using a Taylor expansion type to rapidly calculate the parameter, wherein if the interpolation period interval is smaller, the Taylor expansion type to calculate the parameter can be adopted, the parameter is substituted into the equation to calculate the interpolation point, the first derivative and the second derivative of the interpolation point, the speed error of the parameter is calculated according to the arch Gao Wucha to be supplemented into the speed of the interpolation point to obtain the actual speed, the planning speed of the interpolation point is differed from the actual speed, and when the speed difference is smaller than the specified error epsilon, the next step is carried out, otherwise, the parameter u is compensated and iterated through the ratio of the planning speed to the actual speed until the whole planning is finally completed. When the interpolation period interval is longer, the interpolation precision can be improved by improving the calculation precision and reducing the higher-order error, for example, the first-order Taylor expansion is replaced by the n-order Taylor expansion, wherein n is more than 1, and after n+1 values are calculated, the Adams implicit method or the Dragon library tower method is used for calculating the subsequent interpolation points, so that the precision is improved, and the calculation load is greatly reduced. Meanwhile, when the ratio of the speed to the curvature radius is larger, the proportion correction is iterated and replaced through a dichotomy, so that the convergence performance is improved, and the stability is improved through sacrificing the iteration times.
And according to the speed V planned by the third part, establishing a relation between the speed V and the parameter u with respect to a parameter equation C (u) so as to obtain an interpolation parameter u, and then bringing the interpolation parameter u into the parameter equation C (u) to obtain an interpolation pose. Because the interpolation parameter u is difficult to solve the analysis solution, especially under the higher-order B spline parameter equation; therefore, the first-order Taylor expansion mode is adopted for iteration, and the interpolation parameter u at the previous moment is based i And its relation to velocity V (u i ) The next interpolation parameter u is obtained i+1 ：
Where u is an interpolation parameter, i is an interpolation count, t is time generally t i+1 -t i I.e. a fixed interpolation period Ts,
the relation of velocity V to curve parameter equation C (u) is introduced:
wherein V (u) i ) Planning for the ith interpolationSpeed (m/s), divided by the parameter equation C (u) at u i The two-norm change rate at the point can be obtained to obtain the i interpolation time about the planning speed V (u) i ) The rate of change of the interpolation parameter u.
As shown in FIG. 3, since the interpolation interval Ts cannot be infinitely small, u is as the velocity V is larger and the radius of curvature r is smaller i+1 The less accurate the value of (c), the more generally the errors include iterative numerical errors, expansion truncation errors, and bow height errors. The expansion truncation error is caused by discarding the higher order number, and the expansion order is reduced by increasing the Taylor; the bow height error is also called contour error, which is caused by the fact that the interpolation points of an actual discrete system do linear motion instead of curves, the length of the curve is smaller than that of a fitted curve, and the compensation ER can be calculated through the curvature radius r of the current interpolation point and the actual speed v and is compensated to the actual speed so as to reduce the bow height error.
Calculate C (u) i ) Curvature at
Radius of curvature->
Bow height error->
Corrected actual speed after adding error compensation
When the interpolation interval time Ts is smaller, the interpolation parameter u can be made i Directly correct as
Wherein V (u) i ) The speed is planned for the current interpolation point.
And finally substituting the planned parameter u into a parameter equation C (u) to obtain the planned interpolation pose { x y z rx ry rz }.
Fifth part: the post-processing of planning interpolation is a process of post-processing the planned interpolation points, and after a segment of Cartesian space pose interpolation point set { x y z rx ry rz } of B spline planning is obtained, the point is taken out from the interpolation point set to issue an actuator. Whether the robot enters a force control adjustment module or not can be selected, such as constant force control, force searching or admittance control, and the like, the interpolation points are secondarily modified, then the Cartesian pose of the interpolation points is inversely solved into the joint angle theta according to the DH parameters of the robot, and finally the joint angle theta is issued to each joint for execution.
Finally, it should be noted that: the foregoing is merely a preferred example of the present invention, and the present invention is not limited thereto, but it is to be understood that modifications and equivalents of some of the technical features described in the foregoing embodiments may be made by those skilled in the art, although the present invention has been described in detail with reference to the foregoing embodiments. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (8)
1. A six-axis mechanical arm track planning method based on B-spline is characterized by comprising the following steps:
s1, firstly fitting a terminal motion track, customizing control point parameters and fitting modes, and fitting a corresponding B spline equation according to the control point parameters;
s2, carrying out Simpson numerical integration on the B spline parameter equation obtained in the previous step to obtain the length of each piecewise function curve and the total length of the B spline curve, and simultaneously sampling the curvature of the B spline parameter equation at a given sampling interval to obtain track motion parameters, thereby providing contents for subsequent speed look-ahead planning and interpolation parameter calculation;
s3, planning a motion speed look-ahead, calculating minimum running time according to a sampling speed, dividing and solving a minimum integer through a minimum interpolation period, reversely solving the maximum limiting speed of each segment through the minimum integer interpolation time, and finally convolving twice according to jerk and acceleration limitation to obtain a smooth speed plan;
s4, planning according to a known B spline curve equation and the speed, and solving interpolation parameters and interpolation points to complete planned track interpolation;
and S5, finally, post-processing the planned interpolation points to obtain Cartesian space interpolation pose, and centrally taking the points from the interpolation points to issue an actuator.
2. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: the control point parameters include position, attitude, and weighting factors.
3. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: the B spline parameter equation is:
wherein w is i N+1 weight factors corresponding to the control vertexes, and 1 for the quasi-uniform planning factor; d, d i N+1 control vertices; n (N) i,p (U) is a node vector u= [ U ] 0 ,u 1 ,u 2 ,…,u n+p+1 ]The expression of the ith p-th basis function is as follows:
and finally outputting a curve parameter equation C (u).
4. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: the B spline parameter equation result comprises the curvature radius of the position part and the curvature radius of the gesture part, wherein the unit of the position curvature radius is meter, and the unit of the gesture curvature radius is radian.
5. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: the Simpson adaptive numerical integration formula is:
wherein a and b are upper and lower limits of parameters of the Simpson sampling interval, and C (u) is a curve parameter equation.
6. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: the convolution formula is:
f (tau) in the first convolution is the speed limit obtained by the second partial flow, and the final speed limit is obtained after interpolation interval specification; g (x- τ) is the acceleration signal:
g(x-τ)＝1/(Vmax/Acc/Ts)*ones(1,(Vmax/Acc/Ts))
the above formula Vmax is the maximum speed in a given whole speed plan, acc is the given acceleration, and Ts is the interpolation interval time;
f (tau) in the second convolution is the planning speed obtained after the first convolution;
g (x- τ) is a jerk signal:
g(x-τ)＝1/(Acc/Jerk/Ts)*ones(1,(Acc/Jerk/Ts))
the above equation Acc is given acceleration, jerk is given Jerk, and Ts is interpolation interval.
7. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: in the step S4, the planned trajectory interpolation includes the following steps:
(1) When the interval of the interpolation period is smaller, a first-order Taylor expansion type parameter is adopted, the parameter is substituted into an equation to calculate an interpolation point, a first-order derivative and a second-order derivative of the interpolation point, then the speed error of the interpolation point is calculated according to a bow Gao Wucha to supplement the speed of the interpolation point to obtain an actual speed, then the planning speed of the interpolation point is differenced from the actual speed, when the speed difference is smaller than a specified error epsilon, the next step is carried out, otherwise, the parameter u carries out compensation iteration through the ratio of the planning speed/the actual speed until all the planning is finally completed;
(2) When the interpolation period interval is longer, replacing the first-order Taylor expansion with the n-order Taylor expansion, wherein n is greater than 1, and calculating the subsequent interpolation points by using an Adams implicit method or a Dragon library tower method after n+1 values are calculated;
(3) When the ratio of the speed to the curvature radius is large, adopting a dichotomy iteration method, and sacrificing the iteration times to improve the stability;
(4) Establishing a relation between the speed V planned in the step S3 and a parameter u with respect to a parameter equation C (u) to obtain an interpolation parameter u, further bringing the interpolation parameter u into the parameter equation C (u) to obtain an interpolation pose, and iterating in a first-order Taylor expansion mode;
(5) Interpolation parameter u based on last moment i And its relation to velocity V (u i ) The next interpolation parameter u is obtained i+1 ：
Where u is an interpolation parameter, i is an interpolation count, t is time generally t i+1 -t i I.e. a fixed interpolation period Ts,
the relation of velocity V to curve parameter equation C (u) is introduced:
wherein V (u) i ) For the planning speed at the ith interpolation,dividing parameter equation C (u) by u i The two-norm change rate at the point i is obtained by determining the i interpolation time with respect to the current planned speed V (u i ) The rate of change of the interpolation parameter u;
(6) The expansion cut-off error is reduced by increasing the Taylor expansion order, and the compensation ER is calculated through the curvature radius r of the current interpolation point and the actual speed v, so that the bow height error is reduced by compensating the actual speed;
(7) Calculate C (u) i ) Curvature at
Radius of curvature->
Bow height error->
Corrected actual speed after adding error compensation>
(8) When the interpolation interval time Ts is smaller, the interpolation parameter u is made i Directly correct as
Wherein V (u) i ) And finally substituting the planned parameter u into a parameter equation C (u) to obtain the planned interpolation pose for the current interpolation point planning speed.
8. The six-axis mechanical arm track planning method based on the B spline according to claim 1, wherein the six-axis mechanical arm track planning method based on the B spline is characterized in that: in the step S5, the post-processing flow includes that after a set of cartesian space pose interpolation points of a section of B spline planning is obtained, the point issuing executor is started to be taken from the set of interpolation points, whether the interpolation points enter a force control adjustment module is selected, the interpolation points are modified for the second time, then the cartesian pose of the interpolation points is inversely solved into joint angles theta according to the DH parameters of the robot, and finally the joint angles theta are issued to each joint for execution.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211539346.XA CN116079714A (en) | 2022-12-02 | 2022-12-02 | Six-axis mechanical arm track planning method based on B spline |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211539346.XA CN116079714A (en) | 2022-12-02 | 2022-12-02 | Six-axis mechanical arm track planning method based on B spline |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116079714A true CN116079714A (en) | 2023-05-09 |
Family
ID=86198224
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211539346.XA Pending CN116079714A (en) | 2022-12-02 | 2022-12-02 | Six-axis mechanical arm track planning method based on B spline |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116079714A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116352725A (en) * | 2023-05-23 | 2023-06-30 | 极限人工智能(北京)有限公司 | Three-time three-section type mechanical arm track planning method, system, equipment and medium |
| CN117406669A (en) * | 2023-12-14 | 2024-01-16 | 法奥意威(苏州)机器人系统有限公司 | Interpolation method, interpolation device, electronic equipment and storage medium |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0764620A (en) * | 1993-08-31 | 1995-03-10 | Mitsubishi Electric Corp | Numerical controller |
| US20100305753A1 (en) * | 2009-05-29 | 2010-12-02 | Kuka Roboter Gmbh | Method And Device For Controlling A Manipulator |
| CN103064344A (en) * | 2012-12-25 | 2013-04-24 | 广东省自动化研究所 | Non uniform rational B spline (NURBS) interpolation based speed smooth control method |
| CN104076743A (en) * | 2014-07-07 | 2014-10-01 | 浙江理工大学 | Interpolation type automatically identified interpolation control method |
| CN105629882A (en) * | 2014-11-07 | 2016-06-01 | 中国科学院沈阳计算技术研究所有限公司 | Trigonometric function speed planning method used for spline interpolation |
| DE102017200084A1 (en) * | 2016-12-06 | 2018-06-07 | Robert Bosch Gmbh | A method of providing a motion contour for a manipulator along predetermined landmarks |
| CN109416529A (en) * | 2017-12-19 | 2019-03-01 | 深圳配天智能技术研究院有限公司 | Digital control system, the device with store function and nurbs curve interpolating method |
| CN114131612A (en) * | 2021-12-20 | 2022-03-04 | 中国科学院长春光学精密机械与物理研究所 | Redundant manipulator real-time look-ahead trajectory planning method based on NURBS curve interpolation algorithm |
| CN114237161A (en) * | 2021-11-16 | 2022-03-25 | 华南理工大学 | Industrial robot NURBS curve interpolation method based on digital filtering |
| CN114952852A (en) * | 2022-06-13 | 2022-08-30 | 天津大学 | NURBS curve speed planning method and equipment for robot and storage medium |
| CN115328032A (en) * | 2022-10-11 | 2022-11-11 | 济南邦德激光股份有限公司 | B-spline speed look-ahead control method, device and medium based on S-shaped acceleration and deceleration |
-
2022
- 2022-12-02 CN CN202211539346.XA patent/CN116079714A/en active Pending
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0764620A (en) * | 1993-08-31 | 1995-03-10 | Mitsubishi Electric Corp | Numerical controller |
| US20100305753A1 (en) * | 2009-05-29 | 2010-12-02 | Kuka Roboter Gmbh | Method And Device For Controlling A Manipulator |
| CN103064344A (en) * | 2012-12-25 | 2013-04-24 | 广东省自动化研究所 | Non uniform rational B spline (NURBS) interpolation based speed smooth control method |
| CN104076743A (en) * | 2014-07-07 | 2014-10-01 | 浙江理工大学 | Interpolation type automatically identified interpolation control method |
| CN105629882A (en) * | 2014-11-07 | 2016-06-01 | 中国科学院沈阳计算技术研究所有限公司 | Trigonometric function speed planning method used for spline interpolation |
| DE102017200084A1 (en) * | 2016-12-06 | 2018-06-07 | Robert Bosch Gmbh | A method of providing a motion contour for a manipulator along predetermined landmarks |
| CN109416529A (en) * | 2017-12-19 | 2019-03-01 | 深圳配天智能技术研究院有限公司 | Digital control system, the device with store function and nurbs curve interpolating method |
| CN114237161A (en) * | 2021-11-16 | 2022-03-25 | 华南理工大学 | Industrial robot NURBS curve interpolation method based on digital filtering |
| CN114131612A (en) * | 2021-12-20 | 2022-03-04 | 中国科学院长春光学精密机械与物理研究所 | Redundant manipulator real-time look-ahead trajectory planning method based on NURBS curve interpolation algorithm |
| CN114952852A (en) * | 2022-06-13 | 2022-08-30 | 天津大学 | NURBS curve speed planning method and equipment for robot and storage medium |
| CN115328032A (en) * | 2022-10-11 | 2022-11-11 | 济南邦德激光股份有限公司 | B-spline speed look-ahead control method, device and medium based on S-shaped acceleration and deceleration |
Non-Patent Citations (2)
| Title |
|---|
| 周乐天;姜文刚;: "工业机器人运动目标轨迹规划仿真研究", 计算机仿真, no. 05, 15 May 2017 (2017-05-15), pages 331 - 346 * |
| 张铁;龚文涛;邹焱飚;: "工业机器人实时NURBS插补的仿真和实验研究", 系统仿真学报, no. 04, 8 April 2018 (2018-04-08), pages 1496 - 1510 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116352725A (en) * | 2023-05-23 | 2023-06-30 | 极限人工智能(北京)有限公司 | Three-time three-section type mechanical arm track planning method, system, equipment and medium |
| CN116352725B (en) * | 2023-05-23 | 2023-10-13 | 极限人工智能(北京)有限公司 | Three-time three-section type mechanical arm track planning method, system, equipment and medium |
| CN117406669A (en) * | 2023-12-14 | 2024-01-16 | 法奥意威(苏州)机器人系统有限公司 | Interpolation method, interpolation device, electronic equipment and storage medium |
| CN117406669B (en) * | 2023-12-14 | 2024-04-12 | 法奥意威(苏州)机器人系统有限公司 | Interpolation method, interpolation device, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116079714A (en) | Six-axis mechanical arm track planning method based on B spline | |
| CN107263484B (en) | Robot joint space point-to-point motion trajectory planning method | |
| CN112757306A (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
| CN112417755A (en) | Master-slave mode surgical robot track prediction control method | |
| CN113510709A (en) | Industrial robot pose precision online compensation method based on deep reinforcement learning | |
| CN111650929A (en) | Self-adaptive sliding mode control method and system and mobile robot controller | |
| CN112975992B (en) | Error-controllable robot track synchronous optimization method | |
| CN113001542B (en) | Robot trajectory planning method and device, readable storage medium and robot | |
| CN116061191B (en) | Model prediction control method for active obstacle avoidance of bionic snake-shaped robot | |
| Zhang et al. | Time-optimal trajectory planning of serial manipulator based on adaptive cuckoo search algorithm | |
| CN118259679A (en) | Multi-constraint robot synchronous learning and motion and force hybrid control method and system | |
| CN114290147A (en) | Automobile hub polishing system and method based on cooperation of duplex robot | |
| CN113848809B (en) | Track planning method based on digital filtering | |
| CN116125906A (en) | Motion planning method, device and equipment for numerical control machining and storage medium | |
| CN115716264A (en) | Coating production line optimization method based on robot track optimization | |
| CN112650217B (en) | Robot trajectory tracking strategy dynamic optimization method based on evaluation function | |
| CN114055467A (en) | Space pose online simulation system based on five-degree-of-freedom robot | |
| CN116834009A (en) | Online compensation system for pose errors of distributed virtual-real twin robot | |
| CN113608496B (en) | Spatial path G 2 Transfer fairing method, apparatus and computer readable storage medium | |
| CN114019911B (en) | Curve fitting method based on speed planning | |
| CN113985887A (en) | Method for generating motion trail of differential mobile robot and motion control device | |
| CN113296515A (en) | Explicit model prediction path tracking method for double-independent electrically-driven vehicle | |
| CN112684793A (en) | Trajectory tracking control method for zero-radius over-bending of robot in discrete system | |
| CN110788859A (en) | Controller parameter universe self-adaptive adjustment system | |
| Bu-Hai et al. | NURBS Piecewise Interpolation Algorithm Based on Discrete S-Type Velocity Planning |
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 |