CN105773620A - Track planning and control method of free curve of industrial robot based on double quaternions - Google Patents
Track planning and control method of free curve of industrial robot based on double quaternions Download PDFInfo
- Publication number
- CN105773620A CN105773620A CN201610266117.3A CN201610266117A CN105773620A CN 105773620 A CN105773620 A CN 105773620A CN 201610266117 A CN201610266117 A CN 201610266117A CN 105773620 A CN105773620 A CN 105773620A
- Authority
- CN
- China
- Prior art keywords
- quaternion
- centerdot
- vector
- interpolation
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000004364 calculation method Methods 0.000 claims abstract description 9
- 239000012636 effector Substances 0.000 claims description 31
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000013178 mathematical model Methods 0.000 claims description 18
- 238000000280 densification Methods 0.000 claims description 14
- 230000036544 posture Effects 0.000 claims description 14
- 238000013519 translation Methods 0.000 claims description 14
- 238000006243 chemical reaction Methods 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 3
- 230000004069 differentiation Effects 0.000 claims description 3
- 238000002154 thermal energy analyser detection Methods 0.000 claims description 2
- 230000001133 acceleration Effects 0.000 abstract 1
- 230000007547 defect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 239000004576 sand Substances 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000012994 industrial processing Methods 0.000 description 1
- 238000003754 machining Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a track planning and control method of a free curve of an industrial robot based on double quaternions. Control point data of the Cartesian space are adopted for describing the contour of the space free curve, and meanwhile an Adams differential equation is adopted for NURBS interpolation compacting calculation, and interpolation speed is adjusted in a self-adaption manner with the maximum contour error and maximum acceleration as constraint conditions; and then the position and posture of the robot in the Cartesian space are converted to a four-dimensional space through a short linear segment obtained through interpolation by adopting the double quaternions, the motion trail of the robot is subjected to spherical linear interpolation through hyperspherical rotation, and finally the track of the NURBS free curve of the industrial robot is planned.
Description
Technical Field
The invention relates to a trajectory planning control method of an industrial robot free curve based on multiple quaternions, and belongs to the technical field of robot trajectory planning.
Background
The requirement of modern manufacturing industry on the performance of the robot is higher and higher, and the trajectory planning algorithm of the robot in the task space plays an important role in a robot control system, and the motion performance and efficiency of the tail end of the robot are directly influenced. In addition, in the motion control of the robot, basic straight-line and circular-arc track curves cannot meet the application requirements of industrial processing, and the common B-spline curve, Bezier curve and Clothoid curve cannot describe a standard analytical curve and a free curve by using an accurate and uniform representation method.
Generally, a series of cartesian or joint space points are given in advance for a desired trajectory of a robot, a speed passing through the points or a time between the two points is given, in addition, a maximum speed allowed by the motion of the robot is limited, and then a position trajectory planning and a posture trajectory planning of a robot end effector are respectively realized, while an industrial robot posture trajectory planning usually adopts an euler angle method and an equivalent axis method to interpolate a posture, but the euler angle has the defect of universal deadlock, and the equivalent axis method has the problem that a rotating shaft cannot be determined when the rotating amount is 0. Although the quaternion method can solve the problem of robot attitude track interpolation, other interpolation algorithms are needed for track position interpolation, the computation amount is large, and the real-time requirement of a control system on track planning is influenced.
Disclosure of Invention
The invention aims to overcome the defects of the prior art, provides the NURBS curve path planning control method of the industrial robot based on the multiple quaternion, and can provide an efficient and high-precision control method for realizing the free curve path planning of the industrial robot.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows:
the trajectory planning control method of the industrial robot free curve based on the multiple quaternion comprises the following steps:
1) establishing a mathematical model of a robot end effector space pose based on a multiple quaternion and a free curve mathematical model described by the robot end effector in a task space NURBS;
2) giving a control point sequence D of a free curve described by the robot end effector in a task space NURBS and a corresponding posture R of the control point;
the control point sequence D is represented as: d ═ D0,d1,…,dnN is the number of control points;
the corresponding postures of the control points adopt a posture rotation matrix R3×3Represents;
3) solving the node vector U corresponding to the control point in the control point sequence in the step 2) according to a Hartley-Giardia method, wherein the concrete process is as follows:
for a given control point diI-0, 1, …, n, predefining a k-degree non-uniform rational B-spline curve and determining its node vector U-U0,u1,…,un+k+1]The specific node value in (1) is solved as follows:
taking the repetition degree of the nodes at two ends as k +1, and taking the definition domain of the curve as a standard parameter domain, namely u ∈ [ uk,un+1]=[0,1]Is then u0=u1=…=uk=0,un+1=un+2=…=un+k+1When the node value is 1, the other node values need to be calculated and solved as follows:
the calculation formula is as follows:
in the formula IjTo control the length of each side of a polygon,/j=|dj-dj-1|,
From formula (4):
then all node values can be obtained;
wherein U is a densified node value between two adjacent nodes of the node vector U, and U isiI is 0,1, … …, n + k +1, and represents a specific node value in the node vector U;
4) carrying out densification processing on the node vector U according to an Adams differential equation theoretical algorithm, wherein the specific process is as follows:
the implicit form of the three-step four-order Adams differential equation is expressed as:
wherein T is an interpolation period,are each ui-2、ui-1、ui、ui+1The first derivative of (a);
will be provided withSubstituting the above formula, one can obtain:
ΔLirepresents a control point diThe feed step size of (a);
the method of combining forward and backward difference instead of differentiation is adopted for simplification:
the difference in the backward direction is obtained by the difference,the difference is applied in the forward direction,
the difference is applied in the forward direction,forward differential
Substituting the above formula for formula (7) to obtain:
and further obtaining a simplified Adams differential equation interpolation algorithm iterative formula:
the post-densification node vector may then be obtained, where,represents ui+1A predicted value of (2);
5) according to the mathematical model of the space pose of the robot end effector in the step 1), correcting the densified node vector by adopting a self-adaptive speed control algorithm to finally obtain an optimal densified node vector, wherein the correcting process comprises the following steps:
will be parameterSubstituting the predicted value as the parameter interpolation into the NURBS equation to obtain a corresponding predicted interpolation point:
representing an estimateToThe point of the interpolation is estimated and,
so as to obtain the corresponding estimated feeding step length as follows:
feed step length is estimatedAnd a feed step Δ LiWith a deviation therebetween, by a relative erroriTo show that:
when relative erroriWhen within the allowable range, thenIs the desired p (u)i+1) Otherwise, the correction is carried out according to the following formula until the result is reachediWithin the allowable range:
finally, obtaining an optimal densification node vector;
6) utilizing the optimal densification node vector in the step 5), and finally obtaining the interpolation point position on the curve according to the free curve mathematical model described by the robot end effector in the step 1) in the task space NURBS;
7) carrying out multiple quaternion conversion according to the interpolation point position and the attitude data of the adjacent space curves, and specifically comprising the following steps:
7-1) homogeneous transformation matrix for terminal pose of each robotBTEAs follows:
firstly, rotating the attitude by a matrix R3×3Obtaining a rotation quaternion Q corresponding to the attitude rotation matrix through the conversion relation between the rotation matrix and the quaternion, and simultaneously obtaining a translation vector P ═ Px(u),py(u),pz(u)]T;
7-2) converting the translation vector P of the three-dimensional space into a quaternion of a four-dimensional space, wherein the conversion formula is as follows:
Dp=cos(ψ/2)+sin(ψ/2)v(16)
in the formula, RlIs the large spherical radius of the four-dimensional space, psi ═ P |/RlV is a unit vector on the translation vector, v ═ P/| P |; when | P | ═ 0, v is a zero vector;
7-3) calculating to obtain a multiple quaternion space pose converted from the tail end pose of the robot to a four-dimensional space through the following formulaPart G and part H;
G=DpQ,
in the formula,is a quaternion DpConjugation of (1);
7-4) discretizing the double-rotation track of the multiple quaternion to obtain a series of interpolation multiple quaternion points, wherein the interpolation multiple quaternion points need to be converted into rotation quaternion and translation vector, and the conversion algorithm is as follows:
Q=(G+H)/(2cosψ)(17)
in the formula,
8) and performing inverse kinematics processing on the pose of the robot end effector obtained by interpolation to obtain a joint angle and drive the joint to move.
In the aforementioned step 1) of the method,
the mathematical model of the robot end effector space pose based on the multiple quaternion is as follows:
wherein,representing the multiple quaternion spatial pose of the robot end effector, ξ and η satisfy ξ2=ξ,η2η + η is 1, ξη is 0, and G and H are unit quaternions;
the mathematical model of the free curve described by the robot end effector in the task space NURBS is as follows: any k-th NURBS curve is expressed as a piecewise rational polynomial vector function:
where p (u) represents a position vector, ω, of a free-form curve described by the robot end-effector in task space NURBSiReferred to as weight factors; diIs a free curve control point; n is the number of control points; n is a radical ofi,k(U) is formed by the node vector U ═ U0,u1,…,un+k+1]Decided B-splineBasis functions, expressed by the de boolean-cox recurrence defining formula:
in the formula, provision is made forU is the value of the node between two adjacent nodes of the node vector U, UiAnd i is 0,1, … …, n + k +1, and indicates a specific node value in the node vector U.
The invention achieves the following beneficial effects:
the invention can realize the trajectory planning of the NURBS free curve of the industrial robot in the Cartesian space, and provides the control method which can effectively improve the working efficiency and the working quality of the industrial robot, reduce the speed fluctuation and improve the working environment of the robot.
Drawings
FIG. 1 is a schematic flow chart of a trajectory planning control method of an industrial robot free curve based on multiple quaternions;
FIG. 2 is a schematic diagram of a multiple quaternion representation space straight line segment position of the trajectory planning control method of the industrial robot free curve based on the multiple quaternion;
fig. 3 is a schematic diagram of expressing the attitude of a space straight line segment by a multiple quaternion in the trajectory planning control method of the free curve of the industrial robot based on the multiple quaternion, wherein the arrow direction in the diagram is a rotating vector axis of the attitude expressed by the quaternion.
Detailed Description
The invention is further described below with reference to the accompanying drawings. The following examples are only for illustrating the technical solutions of the present invention more clearly, and the protection scope of the present invention is not limited thereby.
The free curve concept is proposed to describe relatively complex geometries to improve the machining efficiency and accuracy of industrial robots. And a Non-uniform rational B-Spline (NURBS) curve can accurately show the distribution characteristics of control points of a curve model through perspective and can effectively solve the defect that model value points cannot be uniformly distributed.
The multiple quaternion is a new mathematical modeling tool based on Clifford algebra and is developed on the basis of quaternions. The positions and postures in the Cartesian space can be converted into the four-dimensional space by adopting the multiple quaternion, the positions and the postures in the space can be respectively represented, and the positions and the postures in the space can also be represented in a unified mode, so that the poses of the start point and the stop point in the four-dimensional space are subjected to spherical linear interpolation on the motion trail of the robot by using the hypersphere rotation.
As shown in fig. 1, the trajectory planning control method of the industrial robot free curve based on multiple quaternions of the invention comprises the following steps:
establishing a mathematical model of a robot end effector space pose based on a multiple quaternion and a free curve mathematical model described by the robot end effector in a task space NURBS;
the mathematical model of the robot end effector space pose based on the multiple quaternion is as follows:
in the formula,representing the multiple quaternion spatial pose of the robot end effector, ξ and η satisfy ξ2=ξ,η2η + η is 1, ξη is 0, and G and H are unit quaternions.
The mathematical model of the free curve described by the robot end effector in the task space NURBS is as follows: any k-th NURBS curve can be expressed as a piecewise rational polynomial vector function:
where p (u) represents a position vector of a free curve described by the robot end effector in the task space NURBS, i.e., [ p (u) ═ p ]x(u),py(u),pz(u)]T;diAs a free curve control point, i.e. di=[xi,yi,zi]Then the control point sequence D ═ D0,d1,…,dn(i is 0,1, …, n), wherein n is the number of control points; omegaiCalled the weight factor, ωiRespectively connected with the control point di(i-0, 1, …, n) when ω isiWhen 1(i is 0,1, …, n), a k-th NURBS curve degenerates to a k-th B-spline curve;
Ni,k(U) is formed by the node vector U ═ U0,u1,…,un+k+1]The B-spline basis function of the decision is represented by the widely used de-boolean-Cox (debor-Cox) recursion definition formula:
in the formula, provision is made forU is the value of the node between two adjacent nodes of the node vector U, UiAnd i is 0,1, … …, n + k +1, and indicates a specific node value in the node vector U.
The three-dimensional coordinate form of the NURBS free curve is then:
giving a control point sequence D of a free curve described by the robot end effector in a task space NURBS and a corresponding posture R of the control point in the free curve space;
the control point sequence D of the given robot task space free curve is as follows: d ═ D0,d1,…,dn(i is 0,1, …, n), wherein n is the number of control points;
adopting an attitude rotation matrix R corresponding to the attitude3×3The expression of (1);
solving a node vector U corresponding to a free curve control point described by the task space NURBS according to a Hartley-Giardian method;
control point d of the free curve described in this step for a given NURBSiI-0, 1, …, n, predefining a k-degree non-uniform rational B-spline (NURBS) curve, and determining its node vector U-U0,u1,…,un+k+1]The specific node value in (1).
In order to facilitate better control of the behavior of the curve at the curve end points, the repetition degree of the nodes at two ends of the invention is taken as k +1, and the definition domain of the curve is generally taken as a standard parameter domain, namely u ∈ [ uk,un+1]=[0,1]Is then u0=u1=…=uk=0,un+1=un+2=…=un+k+1And (3) calculating and solving the rest inner nodes as 1 by the following method:
the Hartley-Giardia method adopted in the step is independent of the parity of the curve times, a unified calculation formula is adopted, the calculation method is more reasonable, and the calculation formula is as follows:
in the formula IjFor controlling the length of each side of the polygon, i.e. /)j=|dj-dj-1L. From formula (4):
and then all the node values in the node vector corresponding to the control point can be obtained.
Step four, carrying out densification processing on the node vector U according to an Adams differential equation theoretical algorithm;
the parameter densification refers to mapping from a three-dimensional track space to a one-dimensional parameter space, and under a parametric interpolation mode, data densification is represented as a parameter densification process, namely, a feeding step length delta L of the track space is mapped to the parameter space to obtain a parameter increment delta u and a next parameter coordinate: u. ofi+1=ui+Δui。
The Adams (Adams) differential equation adopted in the step is used for compacting the parameter, and the calculation formula is as follows:
the implicit format of the Altemus differential equation, which takes three and four orders, is expressed as:
in the formula, T is an interpolation period,are each ui-2、ui-1、ui、ui+1The first derivative of (a);
will be provided withSubstituting the above formula, one can obtain:
ΔLirepresents a control point diThe feed step size of (2).
In order to ensure the requirement of direct interpolation of a high-speed NURBS curve and the calculation speed of real-time interpolation, the algorithm is simplified by adopting a method of combining forward and backward differences (as shown in the following formula) instead of differentiation:
(backward difference) of the signals,(forward differential),
(forward differential),(Forward difference)
Substituting the above formula for formula (7) to obtain:
and further obtaining a simplified Altemus differential equation interpolation algorithm iterative formula:
the post-densification node vector may then be obtained, where,represents ui+1An estimate of (2).
Fifthly, according to a free curve mathematical model described by the robot end effector in the task space NURBS in the first step, correcting the encrypted node vector by adopting a self-adaptive speed control algorithm, and finally obtaining an optimal encrypted node vector;
will be parameterSubstituting the predicted value as the parameter interpolation into the NURBS equation to obtain a corresponding predicted interpolation point:
it is to estimate the interpolation point of the interpolation,
thereby obtaining the corresponding estimated feeding step lengthComprises the following steps:
estimated feeding step length obtained by using estimation methodAnd the feed step Δ L, can be assessed by the relative error:
irepresents a control point diRelative error of (c).
When relative erroriWhen the temperature is within the allowable range, the temperature can be considered asIs the desired p (u)i+1) Otherwise, the correction is carried out according to the following formula until the result is reachediWithin the allowable range:
and finally obtaining the optimized densified node vector.
And step six, utilizing the optimized densification node vector and finally obtaining the interpolation point position on the curve according to the free curve mathematical model described by the robot end effector in the task space NURBS in the step one.
Seventhly, performing multiple quaternion conversion according to interpolation point positions and attitude data of adjacent space curves;
converting the pose homogeneous transformation matrix into multiple quaternions, and calculating the accuracy of the pose matrix in a given three-dimensional spacemAnd the maximum boundary L of the robot workspace, by the formula:
obtaining the radius R of a large sphere, namely an ultra-large sphere, in a four-dimensional spacel。
The pose homogeneous transformation matrix is converted into a multiple quaternion, and the conversion algorithm is as follows:
7-1) homogeneous transformation matrix for terminal pose of each robotBTEI.e. the centre coordinates of the flange at the end of the robotThe homogeneous transformation matrix of system E with respect to base coordinate system B is as follows:
firstly, rotating the attitude by a matrix R3×3Obtaining a rotation quaternion Q corresponding to the attitude rotation matrix through the conversion relation between the rotation matrix and the quaternion, and simultaneously obtaining a translation vector P ═ Px(u),py(u),pz(u)]T;
7-2) approximately converting the translation vector P of the three-dimensional space into a quaternion of the four-dimensional space, wherein the conversion formula is as follows:
Dp=cos(ψ/2)+sin(ψ/2)v(16)
wherein psi ═ P |/RlV is a unit vector on the translation vector, v ═ P/| P |; when | P | ═ 0, v is a zero vector;
7-3) calculating to obtain a multiple quaternion space pose converted from the tail end pose of the robot to a four-dimensional space through the following formulaPart G and part H;
G=DpQ,
in the formula,is a quaternion DpConjugation of (1);
7-4) discretizing the double-rotation track of the multiple quaternion to obtain a series of interpolation multiple quaternion points, wherein the interpolation multiple quaternion points need to be converted into rotation quaternion and translation vector, and the conversion algorithm is as follows:
Q=(G+H)/(2cosψ)(17)
in the formula,
and step eight, performing inverse kinematics processing on the pose of the robot end effector obtained by interpolation to obtain a joint angle and driving the joint to move.
The specific method comprises the following steps: taking the adjacent interpolation points in the robot task space as the starting and stopping points, the attitude rotation matrix R of the starting and stopping points can be usedsWith corresponding translation vector PsAttitude rotation matrix ReWith corresponding translation vector PeData are respectively converted into multiple quaternion space poses:
and
wherein,rotating matrix R for attitudesCorresponding multiple quaternion spatial pose, GsAnd HsIs composed ofThe corresponding unit quaternion is then calculated,rotating matrix R for attitudeeCorresponding multiple quaternion spatial pose, GeAnd HeIs composed ofThe corresponding unit quaternion.
Respectively carrying out spherical linear interpolation on the G part and the H part of the start-stop point multiple quaternion space pose to obtain:
in the formula, G (t) represents a spherical linear interpolation of the G part of the quaternion spatial pose multiplied by the start point, H (t) represents a spherical linear interpolation of the H part of the quaternion spatial pose multiplied by the start point, and α is arccos (G)s·Ge),β=arccos(Hs·He)(Gs·Ge、Hs·HeAre each GsAnd Ge、HsAnd HeDot product of two quaternions), l (t) ∈ [0,1]May be obtained by normalizing the interpolation period.
From the above equation, the spherical linear interpolation of the multiple quaternion spatial pose can be expressed as:
the formula is a representation method of multiple quaternion spherical linear interpolation, spherical linear interpolation of G part and H part of multiple quaternion is included in the actual interpolation calculation process, the spherical linear interpolation of the multiple quaternion is completed through the spherical linear interpolation of unit quaternion of the G part and the H part respectively, the multiple quaternion of an interpolation intermediate point is obtained, the position and the posture of a linear line section of a multiple quaternion spherical linear interpolation space are respectively shown in figures 2 and 3, and then the multiple quaternion is converted into a pose homogeneous transformation matrix T of the interpolation intermediate point through the inverse process of step seven. And finally, obtaining real-time joint angles through inverse kinematics processing of the robot model, and driving each joint to move.
The above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, several modifications and variations can be made without departing from the technical principle of the present invention, and these modifications and variations should also be regarded as the protection scope of the present invention.
Claims (2)
1. The trajectory planning control method of the industrial robot free curve based on the multiple quaternion is characterized by comprising the following steps of:
1) establishing a mathematical model of a robot end effector space pose based on a multiple quaternion and a free curve mathematical model described by the robot end effector in a task space NURBS;
2) giving a control point sequence D of a free curve described by the robot end effector in a task space NURBS and a corresponding posture R of the control point;
the control point sequence D is represented as: d={d0,d1,···,dnN is the number of control points;
the corresponding postures of the control points adopt a posture rotation matrix R3×3Represents;
3) solving the node vector U corresponding to the control point in the control point sequence in the step 2) according to a Hartley-Giardia method, wherein the concrete process is as follows:
for a given control point diI-0, 1, …, n, predefining a k-degree non-uniform rational B-spline curve and determining its node vector U-U0,u1,…,un+k+1]The specific node value in (1) is solved as follows:
the repetition degree of the nodes at two ends is taken as k +1, and the definition domain of the curve is taken as a standard parameter domain, namely
u∈[uk,un+1]=[0,1]Is then u0=u1=…=uk=0,un+1=un+2=…=un+k+1When the node value is 1, the other node values need to be calculated and solved as follows:
the calculation formula is as follows:
in the formula IjTo control the length of each side of a polygon,/j=|dj-dj-1|,
From formula (4):
then all node values can be obtained;
wherein U is a densified node value between two adjacent nodes of the node vector U, and U isiI is 0,1, … …, n + k +1, and represents a specific node value in the node vector U;
4) carrying out densification processing on the node vector U according to an Adams differential equation theoretical algorithm, wherein the specific process is as follows:
the implicit form of the three-step four-order Adams differential equation is expressed as:
wherein T is an interpolation period,are each ui-2、ui-1、ui、ui+1The first derivative of (a);
will be provided withSubstituting the above formula, one can obtain:
ΔLirepresents a control point diThe feed step size of (a);
the method of combining forward and backward difference instead of differentiation is adopted for simplification:
the difference in the backward direction is obtained by the difference,the difference is applied in the forward direction,
the difference is applied in the forward direction,forward differential
Substituting the above formula for formula (7) to obtain:
and further obtaining a simplified Adams differential equation interpolation algorithm iterative formula:
the post-densification node vector may then be obtained, where,represents ui+1A predicted value of (2);
5) according to the mathematical model of the space pose of the robot end effector in the step 1), correcting the densified node vector by adopting a self-adaptive speed control algorithm to finally obtain an optimal densified node vector, wherein the correcting process comprises the following steps:
will be parameterSubstituting the predicted value as the parameter interpolation into the NURBS equation to obtain a corresponding predicted interpolation point:
representing an estimateThe estimated interpolation point of the interpolation point is estimated,
so as to obtain the corresponding estimated feeding step length as follows:
feed step length is estimatedAnd a feed step Δ LiWith a deviation therebetween, by a relative erroriTo show that:
when relative erroriWhen within the allowable range, thenIs the desired p (u)i+1) Otherwise, the correction is carried out according to the following formula until the result is reachediWithin the allowable range:
finally, obtaining an optimal densification node vector;
6) utilizing the optimal densification node vector in the step 5), and finally obtaining the interpolation point position on the curve according to the free curve mathematical model described by the robot end effector in the step 1) in the task space NURBS;
7) carrying out multiple quaternion conversion according to the interpolation point position and the attitude data of the adjacent space curves, and specifically comprising the following steps:
7-1) homogeneous transformation matrix for terminal pose of each robotBTEAs follows:
firstly, rotating the attitude by a matrix R3×3Obtaining a rotation quaternion Q corresponding to the attitude rotation matrix through the conversion relation between the rotation matrix and the quaternion, and simultaneously obtaining a translation vector P ═ Px(u),py(u),pz(u)]T;
7-2) converting the translation vector P of the three-dimensional space into a quaternion of a four-dimensional space, wherein the conversion formula is as follows:
Dp=cos(ψ/2)+sin(ψ/2)v(16)
in the formula, RlIs the large spherical radius of the four-dimensional space, psi ═ P |/RlV is a unit vector on the translation vector, v ═ P/| P |; when | P | ═ 0, v is a zero vector;
7-3) calculating to obtain a multiple quaternion space pose converted from the tail end pose of the robot to a four-dimensional space through the following formulaPart G and part H;
in the formula,is a quaternion DpConjugation of (1);
7-4) discretizing the double-rotation track of the multiple quaternion to obtain a series of interpolation multiple quaternion points, wherein the interpolation multiple quaternion points need to be converted into rotation quaternion and translation vector, and the conversion algorithm is as follows:
Q=(G+H)/(2cosψ)(17)
in the formula,
8) and performing inverse kinematics processing on the pose of the robot end effector obtained by interpolation to obtain a joint angle and drive the joint to move.
2. The multiple quaternion-based trajectory planning control method for an industrial robot freeform curve according to claim 1, characterized in that in the step 1),
the mathematical model of the robot end effector space pose based on the multiple quaternion is as follows:
wherein,representing the multiple quaternion spatial pose of the robot end effector, ξ and η satisfy ξ2=ξ,η2η + η is 1, ξη is 0, and G and H are unit quaternions;
the mathematical model of the free curve described by the robot end effector in the task space NURBS is as follows: any k-th NURBS curve is expressed as a piecewise rational polynomial vector function:
where p (u) represents a position vector, ω, of a free-form curve described by the robot end-effector in task space NURBSiReferred to as weight factors; diIs a free curve control point; n is the number of control points; n is a radical ofi,k(U) is formed by the node vector U ═ U0,u1,…,un+k+1]The determined B-spline basis function is represented by a DeBoolean-Corx recursion definition formula:
in the formula, provision is made forU is the value of the node between two adjacent nodes of the node vector U, UiAnd i is 0,1, … …, n + k +1, and indicates a specific node value in the node vector U.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610266117.3A CN105773620B (en) | 2016-04-26 | 2016-04-26 | The trajectory planning control method of industrial robot free curve based on Double quaternions |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610266117.3A CN105773620B (en) | 2016-04-26 | 2016-04-26 | The trajectory planning control method of industrial robot free curve based on Double quaternions |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105773620A true CN105773620A (en) | 2016-07-20 |
CN105773620B CN105773620B (en) | 2017-09-12 |
Family
ID=56399369
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610266117.3A Expired - Fee Related CN105773620B (en) | 2016-04-26 | 2016-04-26 | The trajectory planning control method of industrial robot free curve based on Double quaternions |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105773620B (en) |
Cited By (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107263472A (en) * | 2017-06-16 | 2017-10-20 | 广东工业大学 | A kind of robot motion's curve processing method and system |
CN107807612A (en) * | 2017-12-12 | 2018-03-16 | 科德数控股份有限公司 | Digit Control Machine Tool sphere processing method based on quaternary number helix sphere interpolation |
CN108614569A (en) * | 2018-06-22 | 2018-10-02 | 北京敏捷动力科技有限公司 | A method of being used for tracing control controlled motion object |
CN108646667A (en) * | 2018-03-05 | 2018-10-12 | 北京华航唯实机器人科技股份有限公司 | Orbit generation method and device, terminal |
WO2018188276A1 (en) * | 2017-04-09 | 2018-10-18 | 北京工业大学 | Error modeling method for tail-end space curve trajectory of six-degree-of-freedom robot |
CN109531573A (en) * | 2018-12-25 | 2019-03-29 | 珞石(山东)智能科技有限公司 | One kind being based on line transect robot pose smooth path generation method |
CN109664303A (en) * | 2019-02-28 | 2019-04-23 | 武汉工程大学 | A kind of four smooth orbit generation methods of shaft industrial robot B-spline transition type that error is controllable |
CN110757454A (en) * | 2019-10-12 | 2020-02-07 | 广州中国科学院先进技术研究所 | Path planning method and device for cooperative rotation of double robots |
WO2020034403A1 (en) * | 2018-08-16 | 2020-02-20 | 居鹤华 | Axis invariant-based multi-axis robot forward kinematics calculation method |
CN111897216A (en) * | 2020-07-16 | 2020-11-06 | 华中科技大学 | Multi-motion-segment speed planning and interpolation method |
CN111913441A (en) * | 2020-08-06 | 2020-11-10 | 南京工程学院 | Corner smooth transition method based on track mode |
CN112269356A (en) * | 2020-10-27 | 2021-01-26 | 南京溧航仿生产业研究院有限公司 | NURBS track interpolation method for robot |
CN112419415A (en) * | 2020-12-08 | 2021-02-26 | 浙江德尚韵兴医疗科技有限公司 | Ultrasonic scanning method for realizing pose planning based on CRS curve fitting |
CN112698623A (en) * | 2020-12-29 | 2021-04-23 | 南京埃斯顿自动化股份有限公司 | Multi-axis contour control method for multi-axis contour application occasion |
CN113103241A (en) * | 2021-04-29 | 2021-07-13 | 哈尔滨工业大学 | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation |
CN113103239A (en) * | 2021-04-29 | 2021-07-13 | 哈尔滨工业大学 | Robot attitude trajectory generation method and device and storage medium |
CN113103240A (en) * | 2021-04-29 | 2021-07-13 | 哈尔滨工业大学 | Method, device and system for realizing C2 continuous robot trajectory planning |
CN113146631A (en) * | 2020-10-16 | 2021-07-23 | 西安工程大学 | Special-shaped prefabricated body robot needling forming path planning method |
CN113211433A (en) * | 2021-04-21 | 2021-08-06 | 山东科技大学 | Separated visual servo control method based on composite characteristics |
CN113733099A (en) * | 2021-09-28 | 2021-12-03 | 广州大学 | Robot smooth track planning method, computer and storage medium |
CN113997157A (en) * | 2021-11-11 | 2022-02-01 | 吉林大学 | Multi-surface common-body optical curved surface polishing machine tool and method based on macro-micro composite driving |
CN114728418A (en) * | 2019-11-19 | 2022-07-08 | 株式会社日立制作所 | Orbit plan generating device, orbit plan generating method, and orbit plan generating program |
CN114952852A (en) * | 2022-06-13 | 2022-08-30 | 天津大学 | NURBS curve speed planning method and equipment for robot and storage medium |
CN116197917A (en) * | 2023-04-28 | 2023-06-02 | 苏州艾利特机器人有限公司 | Self-adaptive maximum acceleration calculation method and device, storage medium and electronic equipment |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS60262213A (en) * | 1984-06-06 | 1985-12-25 | Nippei Toyama Corp | Movement control method of industrial robot |
JPH05185386A (en) * | 1992-01-16 | 1993-07-27 | Fujitsu Ltd | Linear motion control system for robot finger |
CN102825602A (en) * | 2012-08-21 | 2012-12-19 | 华北电力大学(保定) | PSD (Position Sensitive Detector)-based industrial robot self-calibration method and device |
CN103568012A (en) * | 2013-10-24 | 2014-02-12 | 安徽埃夫特智能装备有限公司 | Method for planning biplanar swing arc track of arc welding robot |
CN104379308A (en) * | 2012-06-29 | 2015-02-25 | 三菱电机株式会社 | Robot control device and robot control method |
CN104635762A (en) * | 2015-01-13 | 2015-05-20 | 北京航空航天大学 | Self-motion angle calculating method facing SRS anthropomorphic arm |
CN104965517A (en) * | 2015-07-07 | 2015-10-07 | 张耀伦 | Robot cartesian space trajectory planning method |
CN105269565A (en) * | 2015-10-30 | 2016-01-27 | 福建长江工业有限公司 | Offline programming and modifying method of six-axis grinding and polishing industrial robot |
-
2016
- 2016-04-26 CN CN201610266117.3A patent/CN105773620B/en not_active Expired - Fee Related
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS60262213A (en) * | 1984-06-06 | 1985-12-25 | Nippei Toyama Corp | Movement control method of industrial robot |
JPH05185386A (en) * | 1992-01-16 | 1993-07-27 | Fujitsu Ltd | Linear motion control system for robot finger |
CN104379308A (en) * | 2012-06-29 | 2015-02-25 | 三菱电机株式会社 | Robot control device and robot control method |
CN102825602A (en) * | 2012-08-21 | 2012-12-19 | 华北电力大学(保定) | PSD (Position Sensitive Detector)-based industrial robot self-calibration method and device |
CN103568012A (en) * | 2013-10-24 | 2014-02-12 | 安徽埃夫特智能装备有限公司 | Method for planning biplanar swing arc track of arc welding robot |
CN104635762A (en) * | 2015-01-13 | 2015-05-20 | 北京航空航天大学 | Self-motion angle calculating method facing SRS anthropomorphic arm |
CN104965517A (en) * | 2015-07-07 | 2015-10-07 | 张耀伦 | Robot cartesian space trajectory planning method |
CN105269565A (en) * | 2015-10-30 | 2016-01-27 | 福建长江工业有限公司 | Offline programming and modifying method of six-axis grinding and polishing industrial robot |
Non-Patent Citations (1)
Title |
---|
季彦伟: "NURBS曲线插补算法的研究与应用", 《中国优秀硕士学位论文全文数据库 工程科技Ⅰ辑 》 * |
Cited By (39)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018188276A1 (en) * | 2017-04-09 | 2018-10-18 | 北京工业大学 | Error modeling method for tail-end space curve trajectory of six-degree-of-freedom robot |
CN107263472A (en) * | 2017-06-16 | 2017-10-20 | 广东工业大学 | A kind of robot motion's curve processing method and system |
CN107807612A (en) * | 2017-12-12 | 2018-03-16 | 科德数控股份有限公司 | Digit Control Machine Tool sphere processing method based on quaternary number helix sphere interpolation |
CN107807612B (en) * | 2017-12-12 | 2020-09-22 | 科德数控股份有限公司 | Numerical control machine tool spherical surface machining method based on quaternion spiral spherical surface interpolation method |
CN108646667A (en) * | 2018-03-05 | 2018-10-12 | 北京华航唯实机器人科技股份有限公司 | Orbit generation method and device, terminal |
CN108646667B (en) * | 2018-03-05 | 2019-11-05 | 北京华航唯实机器人科技股份有限公司 | Orbit generation method and device, terminal |
CN108614569A (en) * | 2018-06-22 | 2018-10-02 | 北京敏捷动力科技有限公司 | A method of being used for tracing control controlled motion object |
CN108614569B (en) * | 2018-06-22 | 2021-01-15 | 北京敏捷动力科技有限公司 | Method for tracking and controlling controlled moving object |
WO2020034403A1 (en) * | 2018-08-16 | 2020-02-20 | 居鹤华 | Axis invariant-based multi-axis robot forward kinematics calculation method |
CN109531573A (en) * | 2018-12-25 | 2019-03-29 | 珞石(山东)智能科技有限公司 | One kind being based on line transect robot pose smooth path generation method |
CN109664303A (en) * | 2019-02-28 | 2019-04-23 | 武汉工程大学 | A kind of four smooth orbit generation methods of shaft industrial robot B-spline transition type that error is controllable |
CN109664303B (en) * | 2019-02-28 | 2021-10-12 | 武汉工程大学 | Error-controllable B-spline transition type smooth trajectory generation method for four-axis industrial robot |
CN110757454A (en) * | 2019-10-12 | 2020-02-07 | 广州中国科学院先进技术研究所 | Path planning method and device for cooperative rotation of double robots |
CN110757454B (en) * | 2019-10-12 | 2022-08-16 | 广州中国科学院先进技术研究所 | Path planning method and device for cooperative rotation of double robots |
CN114728418B (en) * | 2019-11-19 | 2023-09-12 | 株式会社日立制作所 | Track plan generation device, track plan generation method, and recording medium |
CN114728418A (en) * | 2019-11-19 | 2022-07-08 | 株式会社日立制作所 | Orbit plan generating device, orbit plan generating method, and orbit plan generating program |
CN111897216A (en) * | 2020-07-16 | 2020-11-06 | 华中科技大学 | Multi-motion-segment speed planning and interpolation method |
CN111913441A (en) * | 2020-08-06 | 2020-11-10 | 南京工程学院 | Corner smooth transition method based on track mode |
CN111913441B (en) * | 2020-08-06 | 2021-11-09 | 南京工程学院 | Corner smooth transition method based on track mode |
CN113146631A (en) * | 2020-10-16 | 2021-07-23 | 西安工程大学 | Special-shaped prefabricated body robot needling forming path planning method |
CN112269356B (en) * | 2020-10-27 | 2022-03-18 | 南京溧航仿生产业研究院有限公司 | NURBS track interpolation method for robot |
CN112269356A (en) * | 2020-10-27 | 2021-01-26 | 南京溧航仿生产业研究院有限公司 | NURBS track interpolation method for robot |
CN112419415B (en) * | 2020-12-08 | 2022-06-17 | 浙江德尚韵兴医疗科技有限公司 | Ultrasonic scanning method for realizing pose planning based on CRS curve fitting |
CN112419415A (en) * | 2020-12-08 | 2021-02-26 | 浙江德尚韵兴医疗科技有限公司 | Ultrasonic scanning method for realizing pose planning based on CRS curve fitting |
CN112698623B (en) * | 2020-12-29 | 2021-11-05 | 南京埃斯顿自动化股份有限公司 | Multi-axis contour control method for multi-axis contour application occasion |
CN112698623A (en) * | 2020-12-29 | 2021-04-23 | 南京埃斯顿自动化股份有限公司 | Multi-axis contour control method for multi-axis contour application occasion |
CN113211433B (en) * | 2021-04-21 | 2022-09-20 | 山东科技大学 | Separated visual servo control method based on composite characteristics |
CN113211433A (en) * | 2021-04-21 | 2021-08-06 | 山东科技大学 | Separated visual servo control method based on composite characteristics |
CN113103240A (en) * | 2021-04-29 | 2021-07-13 | 哈尔滨工业大学 | Method, device and system for realizing C2 continuous robot trajectory planning |
CN113103239A (en) * | 2021-04-29 | 2021-07-13 | 哈尔滨工业大学 | Robot attitude trajectory generation method and device and storage medium |
CN113103241A (en) * | 2021-04-29 | 2021-07-13 | 哈尔滨工业大学 | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation |
CN113103240B (en) * | 2021-04-29 | 2022-08-23 | 哈尔滨工业大学 | Method, device and system for realizing C2 continuous robot trajectory planning |
CN113103241B (en) * | 2021-04-29 | 2022-08-23 | 哈尔滨工业大学 | Method, device and system for realizing G2 continuous robot double-NURBS track interpolation |
CN113733099A (en) * | 2021-09-28 | 2021-12-03 | 广州大学 | Robot smooth track planning method, computer and storage medium |
CN113997157A (en) * | 2021-11-11 | 2022-02-01 | 吉林大学 | Multi-surface common-body optical curved surface polishing machine tool and method based on macro-micro composite driving |
CN114952852A (en) * | 2022-06-13 | 2022-08-30 | 天津大学 | NURBS curve speed planning method and equipment for robot and storage medium |
CN114952852B (en) * | 2022-06-13 | 2024-09-20 | 天津大学 | Robot NURBS curve speed planning method, equipment and storage medium |
CN116197917A (en) * | 2023-04-28 | 2023-06-02 | 苏州艾利特机器人有限公司 | Self-adaptive maximum acceleration calculation method and device, storage medium and electronic equipment |
CN116197917B (en) * | 2023-04-28 | 2023-08-01 | 苏州艾利特机器人有限公司 | Self-adaptive maximum acceleration calculation method and device, storage medium and electronic equipment |
Also Published As
Publication number | Publication date |
---|---|
CN105773620B (en) | 2017-09-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105773620B (en) | The trajectory planning control method of industrial robot free curve based on Double quaternions | |
CN108000501B (en) | Novel trajectory planning method for series robot | |
CN106647282B (en) | Six-degree-of-freedom robot trajectory planning method considering tail end motion error | |
CN109571473B (en) | Error-controllable small line segment trajectory fairing method | |
CN106393106B (en) | The robot nurbs curve of parameter adaptive densification moves interpolating method | |
KR101056600B1 (en) | Industrial Product Design Method Using Closoid Curve and Numerical Control Method and Apparatus Using Industrial Product Designed by Two Design Method | |
Brooks et al. | Robot-assisted 3D printing of biopolymer thin shells | |
CN107685330A (en) | A kind of Inverse Kinematics Solution method for solving of six degree of freedom wrist bias series robot | |
CN112757306A (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
CN105500354A (en) | Transitional track planning method applied by industrial robot | |
Chen et al. | An approach to the path planning of tube–sphere intersection welds with the robot dedicated to J-groove joints | |
Kabir et al. | Generation of synchronized configuration space trajectories of multi-robot systems | |
CN111459160B (en) | Large-scale track smoothing method for unmanned washing and sweeping vehicle on open road | |
CN104933232B (en) | A kind of kinematics solution method of rod end float type six-degree-of-freedom parallel robot with angle compensation | |
CN110900612A (en) | Pose-synchronous six-axis industrial robot track smoothing method | |
CN106647623A (en) | Geometric precision and linking speed optimization five-axis linkage smooth interpolation method | |
CN107966907A (en) | A kind of Obstacle avoidance applied to redundancy mechanical arm solves method | |
CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
CN115502984A (en) | Six-degree-of-freedom robot side milling motion planning method and system | |
CN109366486B (en) | Flexible robot inverse kinematics solving method, system, equipment and storage medium | |
Zou et al. | A novel robot trajectory planning algorithm based on NURBS velocity adaptive interpolation | |
CN111515954A (en) | Method for generating high-quality motion path of mechanical arm | |
CN114063621B (en) | Wheel type robot formation tracking and obstacle avoidance control method | |
CN114347017B (en) | Curved surface motion control method of adsorption type mobile processing robot based on plane projection | |
CN109531573B (en) | Robot posture smooth path generation method based on sample lines |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20170912 |