CN113103241B - Method, device and system for realizing G2 continuous robot double-NURBS track interpolation - Google Patents
Method, device and system for realizing G2 continuous robot double-NURBS track interpolation Download PDFInfo
- Publication number
- CN113103241B CN113103241B CN202110473012.6A CN202110473012A CN113103241B CN 113103241 B CN113103241 B CN 113103241B CN 202110473012 A CN202110473012 A CN 202110473012A CN 113103241 B CN113103241 B CN 113103241B
- Authority
- CN
- China
- Prior art keywords
- nurbs
- interpolation
- trajectory
- discrete
- track
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention provides a method, a device and a system for realizing double NURBS (non-uniform rational B-spline) track interpolation of a G2 continuous robot, and relates to the technical field of robot track planning control. The method comprises the following steps: acquiring a discrete track of the robot; generating a location NURBS trajectory; dispersing the NURBS locus, determining arc length parameters according to discrete points, and establishing a u-s model; generating an attitude NURBS trajectory; establishing a w-u model; and obtaining discrete pose interpolation points of the double NURBS tracks according to the S-type speed model, the u-S model and the w-u model and a fixed sampling period, and solving the discrete interpolation points through inverse kinematics to obtain the interpolation track of the robot joint space. According to the technical scheme, the double NURBS tracks are adopted, so that the tail end pose track G2 of the robot is ensured to be continuous, the smooth motion is ensured, and all pose points can be continuously passed; by establishing the mapping relation between the position NURBS track parameter and the arc length parameter, the speed fluctuation in the interpolation process is reduced, and the effective planning of the robot track is realized.
Description
Technical Field
The invention relates to the technical field of robot trajectory planning control, in particular to a method, a device and a system for realizing double NURBS trajectory interpolation of a robot with continuous G2.
Background
The robot processing field needs to ensure that the terminal pose trajectory meets the continuity of error constraint and acceleration/impact. The robot track generated by Computer Aided Manufacturing (CAM) software is usually a small line segment track, and the track G0 represented by the small line segment is continuous, G1 and G2 are discontinuous, and the system is accelerated and decelerated frequently, so that the joint wear of the robot is easily caused, and the machining precision and the surface quality of parts are reduced.
The robot end trajectory in cartesian space contains position and attitude information, so the robot trajectory plan contains a plan of positions and attitudes. NURBS (Non-Uniform Rational NURBS) curves are widely used in the NURBS trajectory fitting of robot positions due to their versatility and easy implementation. NURBS is defined by control points and node vectors, with which to represent trajectories that can reduce data storage. In addition, the NURBS has high continuity, G2 continuity can be guaranteed through three times of NURBS curves, and meanwhile, the impact, the over travel, the step loss or the oscillation of the robot caused by speed mutation can be reduced by adopting S-shaped curve speed control.
The terminal attitude trajectory of the robot is required to be sufficiently smooth, so that the trajectory tracking performance of the robot can be improved. According to the difference of the robot gesture description modes, the following methods are mainly adopted: rotation matrix, euler angle, RPY, quaternion, etc. Quaternions are widely applied to robot attitude trajectory fitting due to the advantages of simple description, intuition, calculation amount and the like. Quaternion attitude interpolation mainly comprises: quaternion linear interpolation, (2) spherical linear interpolation, and spherical spline interpolation. However, the spherical linear interpolation only has C0 continuity, and the spherical spline interpolation also only has C1 continuity, which cannot meet the requirement of actual planning. In order to obtain a continuous quaternion spline curve of C2 with higher continuity, the method is provided.
However, the existing method for integrally planning and interpolating the track Point and the attitude track of The Central Point (TCP) of the robot Tool is difficult to simultaneously satisfy the continuous G2 of the position and attitude track of the robot, and the track of the dual NURBS (Non-Uniform Rational NURBS) has the problem of unsynchronized parameters. Meanwhile, the arc length and the parameters of the spline curve are in a nonlinear relation, and the nonlinear relation enables speed fluctuation to be generated in interpolation, so that the normal track of the robot is influenced. The above problems all affect the trajectory planning of the robot.
Disclosure of Invention
The invention solves the problem of how to effectively realize the G2 continuous robot track planning.
In order to solve the above problems, the present invention provides a method for implementing G2 continuous robot dual NURBS trajectory interpolation, including: acquiring a discrete trajectory of the robot, wherein the discrete trajectory is defined by a tool center point { p } i } n i＝0 And quaternion attitude q i } n i＝0 Composition is carried out; for the tool center point { p i } n i＝0 Fitting to generate position NURBSTrack c (u); sampling the position NURBS trajectory c (u) to determine discrete points { c (u) } i ) n i＝0 According to said discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 (ii) a Establishing position NURBS trajectory parameter { u } by cubic spline interpolation i } n i＝0 And the arc length parameter s i } n i＝0 U-s model in between; for the quaternion attitude { q i } n i＝0 Fitting to generate an attitude NURBS track c (w); establishing a w-u model between a position NURBS trajectory parameter u and an attitude NURBS trajectory parameter w by adopting cubic spline interpolation; and dispersing the position NURBS track c (u) and the attitude NURBS track c (w) according to an S-type speed model, the u-S model and the w-u model and a fixed sampling period to obtain discrete interpolation points of double NURBS tracks, and solving the discrete interpolation points through inverse kinematics to obtain an interpolation track of the robot joint space.
According to the robot double-NURBS track interpolation method for realizing G2 continuity, double-NURBS tracks are adopted, so that the terminal pose track G2 of the robot is continuous, smooth movement is guaranteed, and all pose points can be continuously passed; by establishing a mapping relation between the position NURBS track parameter u and the arc length parameter s, the speed fluctuation in the interpolation process is reduced, and the effective planning of the robot track is realized.
Optionally, the pair of tool center points { p } i } n i＝0 Fitting to generate the position NURBS trajectory c (u) includes: using three NURBS curves to align the tool center point { p } i } n i＝0 Fitting to generate the position NURBS trajectory c (u).
According to the double-NURBS track interpolation method for realizing the G2 continuous robot, the three-time NURBS curve is adopted to fit the center point of the tool to generate the position NURBS track c (u), so that the fitting effect of the position NURBS track c (u) is effectively improved, and the effective planning of the robot track is further realized.
Optionally, the sampling of the location NURBS trajectory c (u)Determining a discrete point { c (u) } i ) n i＝0 The method comprises the following steps: isoparametric sampling the positional NURBS trajectory c (u) to determine the discrete point { c (u) } i ) n i＝0 }。
The invention discloses a double-NURBS track interpolation method for realizing G2 continuity of a robot, which is characterized in that a position NURBS track c (u) is subjected to equal-parameter sampling so as to determine discrete points { c (u) } i ) n i＝0 And then, effectively planning the track of the robot.
Optionally, the method is based on the discrete points { c (u) i ) n i＝0 Determine arc length parameter s i } n i＝0 The method comprises the following steps: estimating two adjacent discrete points { c (u) } by using Boolean formula i ) n i＝0 Determining the arc length parameter s according to the arc length i } n i＝0 。
The invention discloses a double-NURBS (non-uniform rational B-spline) track interpolation method for realizing G2 continuity, which is used for accurately estimating two adjacent discrete points { c (u) } by adopting a Boolean formula i ) n i＝0 The arc length between the two, and then the arc length parameter(s) is determined according to the arc length i } n i＝0 The accuracy of the arc length parameter is effectively improved, and then the effective planning of the robot track is realized.
Optionally, the pair of the quaternion poses { q } i } n i＝0 Fitting to generate pose NURBS trajectory c (w) includes: quaternion for each attitude point correspondence:
establishing the quaternion q i And said quaternion mapping vector q i ' mapping relation q i →q i '：
Mapping the vector q according to the quaternion by using cubic NURBS interpolation fitting i ' generating the pose NURBS trajectory c (w).
According to the double-NURBS track interpolation method for realizing G2 continuity of the robot, the attitude NURBS track c (w) is generated through three times of NURBS interpolation fitting, and effective planning of the robot track is further realized.
Optionally, the discretizing the position NURBS trajectory c (u) and the attitude NURBS trajectory c (w) according to an S-type velocity model, the u-S model and the w-u model according to a fixed sampling period to obtain discrete interpolation points of a double NURBS trajectory, and solving the discrete interpolation points through inverse kinematics to obtain an interpolation trajectory of a robot joint space includes: according to the S-shaped speed model, calculating corresponding arc length parameters through the time corresponding to each sampling period point; determining sampling parameters and discrete points corresponding to the position NURBS locus c (u) according to the u-s model and the arc length parameter; according to the w-u model, determining sampling parameters and discrete points corresponding to the quaternion attitude NURBS locus c (w); determining discrete interpolation points corresponding to the double NURBS tracks according to discrete points corresponding to the position NURBS track c (u) and discrete points corresponding to the attitude NURBS track c (w); during interpolation, when calculating discrete points corresponding to the pose NURBS locus c (w), assuming that corresponding sampling parameters are w 0 The sampling parameter w is 0 Substituting the pose NURBS track c (w) to obtain
Calculating parameters
Reversely solving quaternion according to the parameter eta
Q is then w Is the sampling parameter w 0 Corresponding discrete points; and according to a specific robot, performing inverse kinematics solution on the discrete interpolation points to obtain an interpolation track of the robot joint space.
According to the G2 continuous robot double-NURBS track interpolation method, according to an S-shaped speed model, a u-S model and a w-u model, according to a fixed sampling period, a position NURBS track c (u) and a posture NURBS track c (w) are dispersed to obtain discrete interpolation points of the double NURBS tracks, then the discrete interpolation points are solved through inverse kinematics by combining a specific robot to obtain an interpolation track of a robot joint space, and effective planning of the robot track is realized.
Optionally, the tool center point { p } i } n i＝0 And the quaternion attitude { q } i } n i＝0 The number of (a) is greater than or equal to 5.
According to the double-NURBS track interpolation method for realizing the G2 continuous robot, the number of tool center points and quaternion postures is set to be more than or equal to 5, so that the generation of the position NURBS track c (u) and the posture NURBS track c (w) is facilitated, and the effective planning of the robot track is realized.
The invention also provides a robot double-NURBS track interpolation device for realizing G2 continuity, which comprises: an acquisition module for acquiring a discrete trajectory of the robot, wherein the discrete trajectory is defined by a tool center point { p } i } n i＝0 And quaternion attitude q i } n i＝0 Composition is carried out; a position fitting module for fitting the tool center point { p } i } n i＝0 Fitting to generate a NURBS locus c (u); a sampling module for sampling the NURBS locus c (u) to determine discrete points { c (u) } i ) n i＝0 According to said discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 (ii) a A first fitting module for establishing the position NURBS trajectory parameter { u } by cubic spline interpolation i } n i＝0 And the arc length parameter s i } n i＝0 U-s model in between; postureA fitting module for fitting the quaternion attitude
Fitting to generate NURBS trajectory c (w); the second fitting module is used for establishing a w-u model between the position NURBS trajectory parameter u and the posture NURBS trajectory parameter w by adopting cubic spline interpolation; and the interpolation module is used for discretizing the position NURBS track c (u) and the posture NURBS track c (w) according to an S-shaped speed model, the u-S model and the w-u model and a fixed sampling period to obtain discrete interpolation points of double NURBS tracks, and solving the discrete interpolation points through inverse kinematics to obtain an interpolation track of a robot joint space. The robot double-NURBS trajectory interpolation device for realizing G2 continuity has the same advantages as the robot double-NURBS trajectory interpolation method for realizing G2 continuity over the prior art, and is not described herein again.
The invention also provides a robot double-NURBS trajectory interpolation system for realizing G2 continuity, which comprises a computer readable storage medium and a processor, wherein the computer readable storage medium is used for storing a computer program, and the computer program is read by the processor and runs to realize the robot double-NURBS trajectory interpolation method for realizing G2 continuity. The robot double-NURBS trajectory interpolation system for realizing G2 continuity has the same advantages as the robot double-NURBS trajectory interpolation method for realizing G2 continuity over the prior art, and is not described herein again.
The present invention also provides a computer readable storage medium storing a computer program which, when read and executed by a processor, implements the method for implementing G2 continuous robot dual NURBS trajectory interpolation as described above. The computer readable storage medium has the same advantages as the above-mentioned robot dual NURBS trajectory interpolation method for realizing G2 continuity over the prior art, and will not be described herein again.
Drawings
FIG. 1 is a schematic diagram of a G2 continuous robot double NURBS trajectory interpolation method according to an embodiment of the present invention;
FIG. 2 is a 7-segment S-shaped velocity model of an embodiment of the present invention;
FIG. 3 is a dual NURBS curve parameter model according to an embodiment of the present invention;
FIG. 4 is a D-H coordinate system of a robot according to an embodiment of the present invention;
FIG. 5 is a graph of angular displacement, velocity, and acceleration of a joint determined by an inverse solution of a planned trajectory in an embodiment of the present invention.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in detail below.
As shown in fig. 1, an embodiment of the present invention provides a method for implementing G2 continuous robot dual NURBS trajectory interpolation, including: acquiring a discrete trajectory of the robot, wherein the discrete trajectory is defined by a tool center point { p } i } n i＝0 And quaternion attitude { q i } n i＝0 Composition is carried out; for the tool center point { p i } n i＝0 Fitting to generate a position NURBS trajectory c (u); sampling the position NURBS trajectory c (u) to determine discrete points { c (u) } i ) n i＝0 According to said discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 (ii) a Establishing position NURBS locus parameter { u } by cubic spline interpolation i } n i＝0 And the arc length parameter s i } n i＝0 U-s model in between; for the quaternion attitude { q i } n i＝0 Fitting to generate an attitude NURBS track c (w); establishing a w-u model between a position NURBS trajectory parameter u and a posture NURBS trajectory parameter w by adopting cubic spline interpolation; dispersing the position NURBS track c (u) and the posture NURBS track c (w) according to an S-type speed model, the u-S model and the w-u model and a fixed sampling period to obtain discrete interpolation points of double NURBS tracks, and solving the discrete interpolation points through inverse kinematics to obtain an interpolation track of a robot joint space.
In particular, the amount of the solvent to be used,in this embodiment, the method for realizing G2 continuous robot double NURBS trajectory interpolation includes: acquiring discrete trajectory of robot (from tool center point { p) i } n i＝0 And quaternion attitude q i } n i＝0 Composition), wherein the posture of the robot can be represented by a rotation matrix, a rotation vector, a quaternion, an euler angle and the like, and the quaternion is adopted in the embodiment; in the embodiment, a cubic NURBS curve fitting algorithm is adopted, so that the number of control points is equal to that of data points, the number of the control points of the NURBS curve is generally more than or equal to 5, and the tool center point and the quaternion attitude are more than or equal to 5.
For tool center point { p i } n i＝0 And fitting to generate a NURBS locus c (u) of the position, namely fitting the central points of all the tools to generate a NURBS locus, and defining the locus as c (u).
Sampling the position NURBS trajectory c (u) to determine discrete points { c (u) } i ) n i＝0 Generally, equal-parameter sampling is adopted, namely, the NURBS locus c (u) of the position is discretized according to equal parameters, the value range of the parameter u in the c (u) is 0 to 1, and an equal-parameter sampling method is adopted for discretization to determine a discrete point { c (u) is discretized i ) n i＝0 According to the discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 。
Establishing position NURBS trajectory parameter { u } by cubic spline interpolation i } n i＝0 And arc length parameter { s } i } n i＝0 And a u-s model between the two is used for reducing the speed fluctuation in the interpolation process by establishing a mapping relation between a position NURBS track parameter u and an arc length parameter s.
Fitting the quaternion postures to generate an attitude NURBS track c (w), wherein the quaternion corresponding to each posture point is as follows:
because q is i0 Not equal to 0, establishing a mapping relation q i →q i ' is:
then three times of NURBS interpolation fitting is adopted, and the vector q is mapped according to the quaternion i ' generate a NURBS trajectory, define the trajectory as c (w).
And (3) establishing a w-u model between the position NURBS track parameter u and the attitude NURBS track parameter w by adopting cubic spline interpolation, namely a parameter synchronization model, and realizing the synchronous motion of the double NURBS tracks in the interpolation process by establishing the parameter synchronization model.
And according to an S-type speed model, a u-S model and a w-u model, dispersing the double NURBS tracks, namely the position NURBS track c (u) and the attitude NURBS track c (w), according to a fixed sampling period, and determining the discrete interpolation points corresponding to the double NURBS tracks.
And completing the planned double NURBS tracks in the steps to obtain discrete pose interpolation points of the double NURBS tracks, solving the discrete interpolation points through inverse kinematics to obtain interpolation tracks of the joint space of the robot, and accordingly, effectively planning the robot tracks.
In the embodiment, the double NURBS tracks are adopted, so that the whole G2 of the tail end pose track of the robot is ensured to be continuous, the smooth motion is ensured, and all pose points can be continuously passed; by establishing a mapping relation between the position NURBS track parameter u and the arc length parameter s, the speed fluctuation in the interpolation process is reduced, and the effective planning of the robot track is realized.
Optionally, the pair of tool center points { p } i } n i＝0 Fitting to generate the position NURBS trajectory c (u) includes: using three NURBS curves to align the tool center point { p } i } n i＝0 Fitting to generate the position NURBS trajectory c (u).
Specifically, in this embodiment, the tool center point { p } is measured i } n i＝0 Fitting to generate the position NURBS trajectory c (u) includes: three NURBS curves for tool center point p i } n i＝0 Fitting to generate positional NURBS trajectoriesTrace c (u), since the NURBS curve has properties of intuition, locality, convex hull rows, convexity preservation, geometric invariance and reduction of variation, etc., and is convenient for local modification, the three-time NURBS curve is adopted for the tool center point { p } i } n i＝0 The fitting is carried out to generate the position NURBS track c (u), so that the fitting effect of the position NURBS track c (u) can be effectively improved, and the effective planning of the robot track is further realized.
In the embodiment, the three-time NURBS curve is adopted to fit the central point of the tool to generate the position NURBS track c (u), so that the fitting effect of the position NURBS track c (u) is effectively improved, and the effective planning of the robot track is further realized.
Optionally, the sampling of the position NURBS trajectory c (u) determines a discrete point { c (u) } i ) n i＝0 The method comprises the following steps: isoparametric sampling of the positional NURBS trajectory c (u) to determine the discrete point { c (u) } i ) n i＝0 }。
In particular, in the present embodiment, the discrete points { c (u) } are determined by sampling the position NURBS trajectory c (u) }, see the previous description i ) n i＝0 The method comprises the following steps: isoparametric sampling of the location NURBS trajectory c (u) to determine discrete points { c (u) } i ) n i＝0 Discretizing a position NURBS track c (u) according to the equal parameters, wherein the value range of the parameter u in c (u) is 0 to 1, and discretizing by adopting an equal parameter sampling method to determine a discrete point c (u) which is a point i ) n i＝0 }. Determining discrete points c (u) by isoparametric sampling of the position NURBS trajectory c (u) i ) n i＝0 And then effective planning of the robot track is achieved.
In the present embodiment, the discrete point { c (u) } is determined by isoparametric sampling of the location NURBS trajectory c (u) } i ) n i＝0 And then effective planning of the robot track is achieved.
Optionally, said according to said discrete points { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 The method comprises the following steps: estimating two adjacent ones by using Boolean formulaDiscrete points { c (u) i ) n i＝0 Determining the arc length parameter s according to the arc length i } n i＝0 。
Specifically, in the present embodiment, from the discrete points { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 The method comprises the following steps: estimating two adjacent discrete points c (u) by using Boolean formula i ) n i＝0 The arc length between the two, and determining the arc length parameter(s) according to the arc length i } n i＝0 。
Curve c (u) in two parameter intervals [ a, b ]]The above arc length formula is:
because no analytic solution exists when the arc length is calculated by the three-time NURBS curve, the arc length is calculated by using a numerical integration method, and a Boolean formula is adopted at the position:
through the Boolean formula, all two adjacent discrete points { c (u) } can be obtained i ) n i＝0 The corresponding arc length between
The arc length parameter s can be obtained i } n i＝0 ：
Specifically, the arc length is accumulated and unitized in sequence to obtain the arc length parameter { s } i } n i＝0 。
In the present embodiment, two adjacent discrete points { c (u) } are estimated using the Boolean formula i ) n i＝0 The arc length between the two is determined, and then the arc length parameter(s) is determined according to the arc length i } n i＝0 The accuracy of the arc length parameter is effectively improved, and then the effective planning of the robot track is realized.
Optionally, the pair of the quaternion poses { q } i } n i＝0 Fitting to generate pose NURBS trajectory c (w) includes: quaternion for each attitude point correspondence:
establishing the quaternion q i And said quaternion mapping vector q i ' mapping relation q i →q i '：
Mapping the vector q according to the quaternion by using cubic NURBS interpolation fitting i ' generating the pose NURBS trajectory c (w).
Specifically, in the present embodiment, the quaternion attitude { q }is oriented i } n i＝0 Fitting to generate pose NURBS trajectory c (w) includes: the quaternion for each attitude point is:
because q is i0 Not equal to 0, establishing a mapping relation q i →q i ' is:
then adopting three times of NURBS interpolation fitting, and mapping the vector q according to the quaternion i ' generating the attitude track c (w) to further realize the effective planning of the robot track.
In the embodiment, the attitude NURBS locus c (w) is generated by three times of NURBS interpolation fitting, and then the effective planning of the robot locus is realized.
Optionally, the discretizing the position NURBS trajectory c (u) and the attitude NURBS trajectory c (w) according to an S-type velocity model, the u-S model and the w-u model according to a fixed sampling period to obtain discrete interpolation points of a double NURBS trajectory, and solving the discrete interpolation points through inverse kinematics to obtain an interpolation trajectory of a robot joint space includes: according to the S-shaped speed model, calculating corresponding arc length parameters through the time corresponding to each sampling period point; determining sampling parameters and discrete points corresponding to the position NURBS locus c (u) according to the u-s model and the arc length parameter; according to the w-u model, determining sampling parameters and discrete points corresponding to the quaternion attitude NURBS locus c (w); determining discrete interpolation points corresponding to the double NURBS tracks according to discrete points corresponding to the position NURBS track c (u) and discrete points corresponding to the attitude NURBS track c (w); during interpolation, when discrete points corresponding to the attitude NURBS locus c (w) are calculated, the corresponding sampling parameter is assumed to be w 0 The sampling parameter w is 0 Substituting the pose NURBS track c (w) to obtain
Calculating parameters
Reversely solving quaternion according to the parameter eta
Then q is w Is the sampling parameter w 0 Corresponding discrete points; and according to a specific robot, performing inverse kinematics solution on the discrete interpolation points to obtain an interpolation track of the robot joint space.
Specifically, in this embodiment, discretizing the dual NURBS trajectories, that is, the position NURBS trajectory c (u) and the attitude NURBS trajectory c (w), according to the S-type velocity model, the u-S model, and the w-u model and according to a fixed sampling period to obtain interpolation points includes: according to the S-shaped speed model, corresponding arc length parameters are calculated through the time corresponding to each sampling period point, wherein the change schematic diagram of displacement S (t), speed v (t), acceleration a (t) and jerk j (t) along with time t is sequentially shown in a combined graph 2 from top to bottom, and the acceleration curve of the 7 sections of S-shaped speed model is continuous, so that the speed can be smoothly switched, the motor impact is avoided, and the S-shaped speed model can be used for control with higher precision requirements.
Determining sampling parameters and discrete points corresponding to the position NURBS locus c (u) according to the u-s model and the arc length parameter; according to the w-u model, determining sampling parameters and discrete points corresponding to the quaternion attitude NURBS locus c (w); determining discrete interpolation points corresponding to the double NURBS tracks according to discrete points corresponding to the position NURBS track c (u) and discrete points corresponding to the attitude NURBS track c (w);
during interpolation, when calculating discrete points corresponding to the pose NURBS locus c (w), assuming that corresponding sampling parameters are w 0 The sampling parameter w is 0 Substituting the pose NURBS track c (w) to obtain
Since the norm of the generalized quaternion is equal to 1, the parameter is calculated
Reversely solving quaternion according to the parameter eta
Q is then w As a sampling parameter w 0 Corresponding discrete points;
wherein, the analysis of the robot track can know that the position NURBS track c (u) and the posture NURBS track c (w) are in synchronous corresponding relationship, namely the parameter value u epsilon [ u ] of the curve c (u) at any time k ，u k+1 ]When there are parameter values w e [ w ] of the curve c (w) k ，w k+1 ]Wherein u is k And w k The data point parameters corresponding to the raw data points on the NURBS curve, respectively.
And according to the specific robot, performing inverse kinematics solution on the discrete interpolation points to obtain an interpolation track of the robot joint space.
In order to ensure synchronous and smooth motion of the double NURBS curves, a w-u model is established, and is obtained by establishing cubic spline interpolation according to data point parameter values respectively corresponding to a tool central point and a posture point in the double NURBS curves and shown in a figure 3.
In this embodiment, the position NURBS trajectory c (u) and pose NURBS trajectory c (w) are discretized according to an S-type velocity model, a u-S model, and a w-u model to determine discrete pose interpolation points, and then combined with a particular robot, wherein the D-H coordinate system of the particular robot is shown in fig. 4, and the link parameters are shown in table 1. The interpolation track of the joint space obtained by solving the inverse kinematics is shown in fig. 5, and therefore, the acceleration of the robot joint track is continuous, namely G2 is continuous, and effective planning of the robot track is achieved.
TABLE 1
Optionally, the tool center point { p i } n i＝0 And the quaternion attitude { q i } n i＝0 The number of (a) is greater than or equal to 5.
Specifically, in this embodiment, the number of tool center points and quaternion postures is greater than or equal to 5, which is beneficial to generating the position NURBS trajectory c (u) and the posture NURBS trajectory c (w), and thus, effective planning of the robot trajectory is achieved. .
In this embodiment, by setting the number of tool center points and quaternion postures to be greater than or equal to 5, it is beneficial to generate the position NURBS trajectory c (u) and the posture NURBS trajectory c (w), and effective planning of the robot trajectory is achieved.
The invention further provides a double NURBS track interpolation device for realizing G2 continuity, and an acquisition module is used for acquiring discrete tracks of the robot, wherein the discrete tracks are formed by tool center points { p i } n i＝0 And quaternion attitude { q i } n i＝0 Composition is carried out; the method comprises the following steps: the position fitting module is used for fitting the tool central point to generate a NURBS locus c (u); a sampling module for sampling the NURBS locus c (u) to determine discrete points { c (u) } i ) n i＝0 According to said discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 (ii) a A first fitting module for establishing a location NURBS trajectory parameter { u } using cubic spline interpolation i } n i＝0 And the arc length parameter { s } i } n i＝0 U-s model in between; an attitude fitting module for fitting the quaternion attitude
Fitting to generate NURBS trajectory c (w); the second fitting module is used for establishing a w-u model between the position NURBS trajectory parameter u and the posture NURBS trajectory parameter w by adopting cubic spline interpolation; and the interpolation module is used for discretizing the position NURBS track c (u) and the posture NURBS track c (w) according to an S-shaped speed model, the u-S model and the w-u model and a fixed sampling period to obtain discrete interpolation points of double NURBS tracks, and solving the discrete interpolation points through inverse kinematics to obtain an interpolation track of a robot joint space.
Another embodiment of the present invention provides a dual NURBS trajectory interpolation system for realizing G2 continuity for a robot, including a computer readable storage medium and a processor, where a computer program is stored, and the computer program is read and executed by the processor to implement the dual NURBS trajectory interpolation method for realizing G2 continuity for a robot as described above.
Another embodiment of the present invention provides a computer-readable storage medium storing a computer program, which when read and executed by a processor, implements the method for implementing G2 continuous robot dual NURBS trajectory interpolation as described above.
Although the present disclosure has been described above, the scope of the present disclosure is not limited thereto. Various changes and modifications may be effected therein by one of ordinary skill in the pertinent art without departing from the spirit and scope of the present disclosure, and these changes and modifications are intended to be within the scope of the present disclosure.
Claims (9)
1. A method for realizing G2 continuous robot double NURBS track interpolation is characterized by comprising the following steps:
acquiring a discrete trajectory of the robot, wherein the discrete trajectory is defined by a tool center point { p } i } n i＝0 And quaternion attitude { q i } n i＝0 Composition is carried out;
for the tool center point { p i } n i＝0 Fitting to generate a NURBS locus c (u);
sampling the position NURBS trajectory c (u) to determine discrete points { c (u) } i ) n i＝0 According to said discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 ；
Establishing position NURBS trajectory parameter { u } by cubic spline interpolation i } n i＝0 And the arc length parameter s i } n i＝0 U-s model in between;
for the quaternion attitude { q i } n i＝0 Fitting to generate an attitude NURBS track c (w);
establishing a w-u model between a position NURBS trajectory parameter u and an attitude NURBS trajectory parameter w by adopting cubic spline interpolation;
dispersing the position NURBS track c (u) and the attitude NURBS track c (w) according to an S-type speed model, the u-S model and the w-u model and a fixed sampling period to obtain discrete interpolation points of double NURBS tracks, solving the discrete interpolation points through inverse kinematics to obtain an interpolation track of a robot joint space, and specifically comprising the following steps:
according to the S-shaped speed model, calculating corresponding arc length parameters through the time corresponding to each sampling period point;
according to the u-s model and the arc length parameter, determining sampling parameters and discrete points corresponding to the NURBS locus c (u);
according to the w-u model, determining sampling parameters and discrete points corresponding to the quaternion attitude NURBS locus c (w);
determining discrete interpolation points corresponding to the double NURBS trajectories according to discrete points corresponding to the position NURBS trajectories c (u) and discrete points corresponding to the attitude NURBS trajectories c (w);
during interpolation, when calculating discrete points corresponding to the pose NURBS locus c (w), assuming that corresponding sampling parameters are w 0 The sampling parameter w is 0 Substituting the pose NURBS track c (w) to obtain
Calculating parameters
Reversely solving quaternion according to the parameter eta
Then q is w Is the sampling parameter w 0 Corresponding discrete points;
and according to a specific robot, performing inverse kinematics solution on the discrete interpolation points to obtain an interpolation track of the robot joint space.
2. The method of claim 1, wherein the pair of tool center points { p } is used for interpolation of G2 continuous robot double NURBS trajectory i } n i＝0 Fitting to generate the position NURBS trajectory c (u) includes:
using three NURBS curves to the toolHaving a center point { p i } n i＝0 Fitting to generate the position NURBS trajectory c (u).
3. The method of claim 1, wherein the NURBS locus c (u) is sampled to determine discrete points { c (u) according to the interpolation method of the G2 continuous robot double NURBS locus i ) n i＝0 The method comprises the following steps:
isoparametric sampling of the positional NURBS trajectory c (u) to determine the discrete point { c (u) } i ) n i＝0 }。
4. The method of claim 1, wherein the interpolation is based on the discrete points { c (u) } according to the dual NURBS trajectory interpolation method for realizing G2 continuity i ) n i＝0 Determine arc length parameter s i } n i＝0 The method comprises the following steps:
estimating two adjacent discrete points { { c (u) { (u) } by using Boolean formula i ) n i＝0 } determining said arc length parameter { s } from said arc length i } n i＝0 。
5. The method of claim 1, wherein the pair of quaternion poses { q } q are used in the interpolation of G2 continuous dual NURBS trajectories i } n i＝0 Fitting to generate pose NURBS trajectory c (w) includes:
quaternion for each attitude point correspondence:
establishing the quaternion q i With said quaternion mapping vector q i ' mapping relation q i →q i '：
Mapping the vector q according to the quaternion by using cubic NURBS interpolation fitting i ' generating the pose NURBS trajectory c (w).
6. The method of claim 1, wherein the tool center point { p } is used for interpolation of G2 continuous robot double NURBS trajectory i } n i＝0 And the quaternion attitude { q i } n i＝0 The number of (a) is greater than or equal to 5.
7. A robot double NURBS trajectory interpolation device for realizing G2 continuity, characterized by comprising:
an acquisition module for acquiring a discrete trajectory of the robot, wherein the discrete trajectory is defined by a tool center point { p } i } n i＝0 And quaternion attitude { q i } n i＝0 Forming;
a position fitting module for fitting the tool center point { p } i } n i＝0 Fitting to generate a NURBS locus c (u);
a sampling module for sampling the NURBS locus c (u) to determine discrete points { c (u) } i ) n i＝0 According to said discrete point { c (u) } i ) n i＝0 Determine arc length parameter s i } n i＝0 ；
A first fitting module for establishing the position NURBS trajectory parameter { u } by cubic spline interpolation i } n i＝0 And the arc length parameter s i } n i＝0 U-s model in between;
an attitude fitting module for fitting the quaternion attitude { q i } n i＝0 Fitting to generate NURBS trajectory c (w);
the second fitting module is used for establishing a w-u model between the position NURBS trajectory parameter u and the attitude NURBS trajectory parameter w by adopting cubic spline interpolation;
the interpolation module is used for discretizing the position NURBS track c (u) and the posture NURBS track c (w) according to an S-shaped speed model, the u-S model and the w-u model and a fixed sampling period to obtain discrete interpolation points of double NURBS tracks, solving the discrete interpolation points through inverse kinematics to obtain an interpolation track of a robot joint space, and specifically comprises the following steps:
according to the S-shaped speed model, calculating corresponding arc length parameters through the time corresponding to each sampling period point;
according to the u-s model and the arc length parameter, determining sampling parameters and discrete points corresponding to the NURBS locus c (u);
according to the w-u model, determining sampling parameters and discrete points corresponding to the quaternion attitude NURBS locus c (w);
determining discrete interpolation points corresponding to the double NURBS tracks according to discrete points corresponding to the position NURBS track c (u) and discrete points corresponding to the attitude NURBS track c (w);
during interpolation, when calculating discrete points corresponding to the pose NURBS locus c (w), assuming that corresponding sampling parameters are w 0 The sampling parameter w is measured 0 Substituting the pose NURBS track c (w) to obtain
Calculating parameters
Reversely solving quaternion q according to the parameter eta w ＝η+
Then q is w Is the sampling parameter w 0 Corresponding discrete points;
and according to a specific robot, performing inverse kinematics solution on the discrete interpolation points to obtain an interpolation track of the robot joint space.
8. A dual NURBS trajectory interpolation system for G2 continuum robot, comprising a computer readable storage medium and a processor storing a computer program, the computer program being readable and executable by the processor for performing the method of any of claims 1 to 6 for performing G2 continuum robot dual NURBS trajectory interpolation.
9. A computer-readable storage medium, characterized in that it stores a computer program which, when read and executed by a processor, implements the method of any of claims 1 to 6 for implementing the G2 continuous robot dual NURBS trajectory interpolation method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110473012.6A CN113103241B (en) | 2021-04-29 | 2021-04-29 | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110473012.6A CN113103241B (en) | 2021-04-29 | 2021-04-29 | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113103241A CN113103241A (en) | 2021-07-13 |
| CN113103241B true CN113103241B (en) | 2022-08-23 |
Family
ID=76720439
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110473012.6A Active CN113103241B (en) | 2021-04-29 | 2021-04-29 | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113103241B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105773620A (en) * | 2016-04-26 | 2016-07-20 | 南京工程学院 | Track planning and control method of free curve of industrial robot based on double quaternions |
| CN110686595A (en) * | 2019-09-27 | 2020-01-14 | 天津大学 | Laser beam space pose calibration method of non-orthogonal axis system laser total station |
| CN112269356A (en) * | 2020-10-27 | 2021-01-26 | 南京溧航仿生产业研究院有限公司 | NURBS track interpolation method for robot |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5426728B2 (en) * | 2012-06-28 | 2014-02-26 | ファナック株式会社 | Numerical control device with tool attitude control function for multi-axis machines |
| US10282914B1 (en) * | 2015-07-17 | 2019-05-07 | Bao Tran | Systems and methods for computer assisted operation |
| CN106393106B (en) * | 2016-09-13 | 2018-11-06 | 东南大学 | The robot nurbs curve of parameter adaptive densification moves interpolating method |
| US11188091B2 (en) * | 2018-03-06 | 2021-11-30 | Zoox, Inc. | Mesh decimation based on semantic information |
| CN111452033A (en) * | 2019-01-18 | 2020-07-28 | 兰州交通大学 | Double NURBS curve milling trajectory planning method for industrial robot |
| CN110083160B (en) * | 2019-05-16 | 2022-04-19 | 哈尔滨工业大学（深圳） | Robot trajectory planning method based on deep learning |
| CN110948493B (en) * | 2019-12-26 | 2021-03-19 | 广州机械科学研究院有限公司 | Robot curve path generation method, system, device and storage medium |
| CN111444467B (en) * | 2020-04-21 | 2022-03-15 | 南京大学 | Method for local linear interpolation and prediction based on real-time positioning track data |
| CN111791236B (en) * | 2020-07-17 | 2021-06-01 | 广东拓斯达科技股份有限公司 | Industrial robot Cartesian space trajectory transition method |
| CN111736537A (en) * | 2020-07-21 | 2020-10-02 | 天津大学 | Method for calculating double NURBS path limit speed in free-form surface machining |
| CN112099493B (en) * | 2020-08-31 | 2021-11-19 | 西安交通大学 | Autonomous mobile robot trajectory planning method, system and equipment |
-
2021
- 2021-04-29 CN CN202110473012.6A patent/CN113103241B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105773620A (en) * | 2016-04-26 | 2016-07-20 | 南京工程学院 | Track planning and control method of free curve of industrial robot based on double quaternions |
| CN110686595A (en) * | 2019-09-27 | 2020-01-14 | 天津大学 | Laser beam space pose calibration method of non-orthogonal axis system laser total station |
| CN112269356A (en) * | 2020-10-27 | 2021-01-26 | 南京溧航仿生产业研究院有限公司 | NURBS track interpolation method for robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113103241A (en) | 2021-07-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107363813B (en) | Desktop industrial robot teaching system and method based on wearable equipment | |
| US11667035B2 (en) | Path-modifying control system managing robot singularities | |
| US8774965B2 (en) | Method and device for controlling a manipulator | |
| JP2019517929A (en) | Trajectory planning method of point-to-point movement in robot joint space | |
| CN112975992B (en) | Error-controllable robot track synchronous optimization method | |
| CN113103240B (en) | Method, device and system for realizing C2 continuous robot trajectory planning | |
| JP3349652B2 (en) | Offline teaching method | |
| Zhang et al. | Robotic curved surface tracking with a neural network for angle identification and constant force control based on reinforcement learning | |
| Guilamo et al. | Manipulability optimization for trajectory generation | |
| CN114571452A (en) | Industrial robot trajectory planning method, electronic device and readable storage medium | |
| Peng et al. | An analytical method for decoupled local smoothing of linear paths in industrial robots | |
| Lu et al. | High-order joint-smooth trajectory planning method considering tool-orientation constraints and singularity avoidance for robot surface machining | |
| CN113001542B (en) | Robot trajectory planning method and device, readable storage medium and robot | |
| CN112356032B (en) | Posture smooth transition method and system | |
| CN113103241B (en) | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation | |
| CN111515954B (en) | Method for generating high-quality motion path of mechanical arm | |
| Xu et al. | Automatic interpolation algorithm for NURBS trajectory of shoe sole spraying based on 7-DOF robot | |
| CN113618728A (en) | Method and device for compensating motion trail of robot and computer storage medium | |
| CN114670177B (en) | Gesture planning method for two-to-one-movement parallel robot | |
| Kim et al. | Development of a trajectory generation method for a five-axis NC machine | |
| CN113276116B (en) | Error-controllable robot track synchronous transition method | |
| CN113608496B (en) | Spatial path G 2 Transfer fairing method, apparatus and computer readable storage medium | |
| CN115808904A (en) | Industrial robot arc auxiliary point passing track planning method | |
| CN114734435A (en) | Encoder calibration method, device and system based on hypersphere | |
| JP2000112510A (en) | Robot teaching method and its device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |