CN112356032B - Posture smooth transition method and system - Google Patents

Posture smooth transition method and system Download PDF

Info

Publication number
CN112356032B
CN112356032B CN202011225643.8A CN202011225643A CN112356032B CN 112356032 B CN112356032 B CN 112356032B CN 202011225643 A CN202011225643 A CN 202011225643A CN 112356032 B CN112356032 B CN 112356032B
Authority
CN
China
Prior art keywords
attitude
curve
formula
transition
parameter
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
Application number
CN202011225643.8A
Other languages
Chinese (zh)
Other versions
CN112356032A (en
Inventor
王�华
郭庆洪
吴自翔
于振中
李文兴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HRG International Institute for Research and Innovation
Original Assignee
HRG International Institute for Research and Innovation
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HRG International Institute for Research and Innovation filed Critical HRG International Institute for Research and Innovation
Priority to CN202011225643.8A priority Critical patent/CN112356032B/en
Publication of CN112356032A publication Critical patent/CN112356032A/en
Application granted granted Critical
Publication of CN112356032B publication Critical patent/CN112356032B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/15—Correlation function computation including computation of convolution operations

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Algebra (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

The invention provides an attitude smooth transition method, a constructed attitude transition curve and a front and rear attitude interpolation curve have two-order geometric continuity at a joint, and the angular velocity smoothness and the angular acceleration continuity at each place can be realized; in addition, the time derivative of the parameter in the transition curve parameter equation is equal to the angular velocity, so that the trajectory planning can be conveniently carried out according to the change rule of the angular velocity.

Description

Posture smooth transition method and system
Technical Field
The invention relates to the technical field of robots, in particular to a posture smooth transition method and system.
Background
The robot programming system provides several basic motion instructions, such as linear motion, circular motion and the like. The robot executes the movement instructions one by one, and the tool center point moves along the path defined by the instructions. If not, the speed of the robot at the target point of each instruction is zero. For some applications, it is desirable to maintain motion of the robot between adjacent commands and to allow the robot to deviate from the path within a certain range for efficiency. One typical approach to this problem is: a transition path is inserted between paths defined by adjacent instructions. In order to ensure stable movement and avoid the impact of acceleration jump on the machine body, the transition path and the front and rear path sections have two-step and more than two-step geometric continuity at the joint.
The path contains two parts of information, namely position and attitude, and correspondingly, the transition path contains position transition and attitude transition. The position interpolation methods of different motion instructions are different, but the attitude interpolation generally adopts spherical linear interpolation in a unified way. The attitude sequence generated by the spherical linear interpolation can be described by a large circular arc on a unit spherical surface in a 4-dimensional space, and the attitude transition is to construct a curve on the spherical surface and complete smooth transition between two large circular arcs. In contrast, the position transition is to construct a satisfactory transition curve in the euclidean space. For attitude transition, it is a difficult task to construct a satisfactory transition curve on a spherical surface rather than in the euclidean space.
According to the pose synchronization six-axis industrial robot track smoothing method disclosed by the application number 201911300865.9, an arc curve is adopted for position track transition, and a quaternion B spline is adopted for pose track transition. The position and the attitude track after transition have high-order continuity, the transition errors of the position and the attitude can be restrained simultaneously, and the position track and the attitude track after transition have parameter synchronism. The invention adopts a B-spline with 5 control points to construct an attitude transition curve, the constructed attitude transition curve and a front and rear attitude interpolation curve have two-order geometric continuity at the joint, but the parameters of the transition curve have no definite meaning to the time derivative, and the trajectory planning is difficult to be carried out according to the change rule of the angular velocity.
Disclosure of Invention
The invention aims to provide a posture smooth transition method.
The invention solves the technical problems through the following technical means:
an attitude smooth transition method comprises the following steps:
s01, according to given three postures q0，q1，q2Selecting a reference coordinate system, and determining a reference coordinate system by q according to a spherical linear interpolation method0To q1Attitude interpolation curve q of01(s) and q1To q2Attitude interpolation curve q of12(s); interpolating the curve q in the pose according to a given angle alpha01(s) selecting the starting attitude q of the transition curveiInterpolating the curve q in attitude12(s) selecting the terminal attitude q of the transition curvef；
S02, constructing and solving a parameter equation q(s) of the transition curve.
In step S01, the method of selecting the reference coordinate system is as follows: remember u0，α0Is composed of q0To q1Rotation axis and rotation angle of u1，α1Is composed of q1To q2Screw ofRotation axis and angle of rotation u0And u1Is recorded as beta, three coordinate vectors i, j, k of the reference coordinate system are determined by the following formula
Figure GDA0003515335220000021
From this, a reference coordinate system can be determined, in which the axis of rotation u0、u1Is shown as
Figure GDA0003515335220000022
In step S01, in the selected reference coordinate system, the attitude is expressed by unit quaternion according to the spherical linear interpolation method, and the curve q is interpolated01(s) and q12(s) can be represented as
Figure GDA0003515335220000023
Wherein
Figure GDA0003515335220000024
p01，p12Respectively represent by q1To q01(s) and the compound represented by the formula q1To q12(s) and the rotational transformation is also expressed by a unit quaternion.
In step S01, a given α satisfies α > 0, α < α0And alpha is less than alpha1Attitude q of originiAnd terminal attitude qfIs selected according to the following formula
Figure GDA0003515335220000025
In step S02, the configuration of the transition curve and the parametric equation solving process are as follows: the parametric equation q(s) for the transition curve is first expressed in the form:
q(s)＝p(s)q1，s∈[0，σ] (2)
wherein s is a parameter of the transition curve, σ is a maximum value of the parameter s, q(s) is any attitude on the transition curve, and p(s) is a composite of q1The rotation transformation to any attitude q(s) on the transition curve is also expressed by unit quaternion, and the writing component is in the form of:
p(s)＝(p0(s)，(p1(s)，p2(s)，p3(s)))
determining the components of p(s), wherein the last component p3(s) is constantly 0, and the remaining three components have the following formal expressions
Figure GDA0003515335220000031
In the formula (3), eta, thetamFor constants related to alpha and beta, the calculation formula is
Figure GDA0003515335220000032
Theta(s), f(s) is a function of s and is expressed as
Figure GDA0003515335220000033
In equation (5), sd, am, and Π are three specific functions related to the elliptic integral: sd (u, m) corresponds to a Jacobi elliptic function sd (u | m), am (u, m) corresponds to a Jacobi amplitude function am (u | m), Π (n; φ, m) corresponds to a third type of incomplete elliptic integral Π (n; φ | m), m is a parameter of the elliptic integral, n is a characteristic number of the elliptic integral, and n and m have the following relations:
Figure GDA0003515335220000034
cψis a common relation with m, nA number, whose expression is
Figure GDA0003515335220000035
Solving for m again according to the formula θ (K (m)/cψ)＝θmSolving for m, i.e. solving equations
Figure GDA0003515335220000036
In the formula (8), K (m) is a first kind of complete elliptic integral, and pi (n; m) is a third kind of complete elliptic integral; from equations (7) and (6), equation (8) is bounded on both sides by m only, using a numerical method in the interval [0, m ]max]The equation for m determined by the above solution (8), wherein
Figure GDA0003515335220000041
Finally, a parameter equation q(s) of the transition curve is determined, and n and c can be solved according to the formula (6) and the formula (7) by the solved mψThen, according to the formula (5) and the formula (3), each component of p(s) and(s) can be determined, and finally, according to the formula (2), a parameter equation of the transition curve can be determined, wherein the maximum value sigma of the parameter s is determined by the following formula:
σ＝2K(m)/cψ (10)
the components of p(s) determined according to the formulas (3) to (8) can ensure the constructed transition curve q(s) and the front-rear attitude interpolation curve q01(s)、q12(s) are each at qiAnd q isfSplicing, and having two-step geometric continuity at the splicing position; in addition, each component p(s) determined by the formulas (3) to (8) can also ensure that the derivative of the parameter s of the transition curve q(s) to time is equal to the angular velocity, so that the trajectory planning can be conveniently carried out according to the change rule of the angular velocity.
The invention also provides a posture smooth transition system, which comprises
A transition curve starting point attitude and end point attitude determination module for determining three given attitudes q0，q1，q2Selecting a reference coordinate system, and determining a reference coordinate system by q according to a spherical linear interpolation method0To q1Attitude interpolation curve q of01(s) and q1To q2Attitude interpolation curve q of12(s); interpolating the curve q in the pose according to a given angle alpha01(s) selecting the starting attitude q of the transition curveiInterpolating the curve q in attitude12(s) selecting the terminal attitude q of the transition curvef；
And the parameter equation constructing and solving module is used for constructing and solving a parameter equation q(s) of the transition curve.
Further, in the interpolation curve determining module, the method for selecting the reference coordinate system is as follows: remember u0，α0Is composed of q0To q1Rotation axis and rotation angle of u1，α1Is composed of q1To q2Rotation axis and rotation angle of u0And u1Is denoted as beta, the three coordinate vectors i, j, k of the reference coordinate system are determined by the following formula
Figure GDA0003515335220000042
From this, a reference coordinate system can be determined, in which the axis of rotation u0、u1Is shown as
Figure GDA0003515335220000043
Furthermore, in the interpolation curve determining module, under the selected reference coordinate system, according to a spherical linear interpolation method, the attitude is represented by a unit quaternion, and an interpolation curve q is represented by a unit quaternion01(s) and q12(s) can be represented as
Figure GDA0003515335220000051
Wherein
Figure GDA0003515335220000052
p01，p12Respectively represent by q1To q01(s) and the compound represented by the formula q1To q12(s) and the rotational transformation is also expressed by a unit quaternion.
Further, in the transition curve starting point posture and end point posture determination module, the given alpha satisfies alpha > 0 and alpha < alpha0And alpha is less than alpha1Attitude q of originiAnd terminal attitude qfIs selected according to the following formula
Figure GDA0003515335220000053
Further, in the parameter equation constructing and solving module, the transition curve constructing and parameter equation solving process is as follows: the parametric equation q(s) for the transition curve is first expressed in the form:
q(s)＝p(s)q1，s∈[0，σ] (2)
wherein s is a parameter of the transition curve, σ is a maximum value of the parameter s, q(s) is any attitude on the transition curve, and p(s) is a composite of q1The rotation transformation to any attitude q(s) on the transition curve is also expressed by unit quaternion, and the writing component is in the form of:
p(s)＝(p0(s)，(p1(s)，p2(s)，p3(s)))
determining the components of p(s), wherein the last component p3(s) is constantly 0, and the remaining three components have the following formal expressions
Figure GDA0003515335220000054
In the formula (3), eta, thetamFor constants related to alpha and beta, the calculation formula is
Figure GDA0003515335220000055
Theta(s), f(s) is a function of s and is expressed as
Figure GDA0003515335220000061
In equation (5), sd, am, and Π are three specific functions related to the elliptic integral: sd (u, m) corresponds to a Jacobi elliptic function sd (u | m), am (u, m) corresponds to a Jacobi amplitude function am (u | m), Π (n; φ, m) corresponds to a third type of incomplete elliptic integral Π (n; φ | m), m is a parameter of the elliptic integral, n is a characteristic number of the elliptic integral, and n and m have the following relations:
Figure GDA0003515335220000062
cψis a constant related to m, n and has the expression of
Figure GDA0003515335220000063
Solving for m again according to the formula θ (K (m)/cψ)＝θmSolving for m, i.e. solving equations
Figure GDA0003515335220000064
In the formula (8), K (m) is a first kind of complete elliptic integral, and pi (n; m) is a third kind of complete elliptic integral; from equations (7) and (6), equation (8) is bounded on both sides by m only, using a numerical method in the interval [0, m ]max]The equation for m determined by the above solution (8), wherein
Figure GDA0003515335220000065
Finally, a parameter equation q(s) of the transition curve is determined, and n and c can be solved according to the formula (6) and the formula (7) by the solved mψThen, each component p(s) and(s) can be determined according to the formula (5) and the formula (3), finally, according to the formula (2), a parameter equation of the transition curve can be determined, and the maximum value sigma of the parameter s is determined by the following formula:
σ＝2K(m)/cψ (10)
the components of p(s) determined according to the formulas (3) to (8) can ensure the constructed transition curve q(s) and the front-rear attitude interpolation curve q01(s)、q12(s) are each at qiAnd q isfSplicing, and having two-step geometric continuity at the splicing position; in addition, each component p(s) determined by the formulas (3) to (8) can also ensure that the derivative of the parameter s of the transition curve q(s) to time is equal to the angular velocity, so that the trajectory planning can be conveniently carried out according to the change rule of the angular velocity.
The invention has the advantages that:
the transition curve q(s) and the front-back attitude interpolation curve q constructed according to the method provided by the invention01(s) and q12(s) the joint has two-order geometric continuity, so that angular velocity everywhere smoothness and angular acceleration everywhere continuity can be realized; and the derivative of the parameter s of the transition curve q(s) to time is equal to the angular velocity, so that the trajectory planning is conveniently carried out according to the change rule of the angular velocity.
Drawings
FIG. 1 is a diagram illustrating solution m in example 2 of the present invention;
FIG. 2 is a schematic diagram of an attitude transition curve in embodiment 2 of the present invention;
FIG. 3 is a graph showing the change of angular velocity with time in example 3 of the present invention;
FIG. 4 is a graph showing the change of angular acceleration with time in embodiment 3 of the present invention;
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the embodiments of the present invention, and it is obvious that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be obtained by a person skilled in the art without inventive step based on the embodiments of the present invention, are within the scope of protection of the present invention.
Example 1
The present embodiment provides a detailed description of the pose smooth transition method.
An attitude smooth transition method, three known attitudes q0，q1，q2Wherein by the attitude q0To q1Has a rotation axis of u0Angle of rotation alpha0Is greater than 0; from q1To q2Has a rotation axis of u1Angle of rotation alpha1＞0；u0And u1The included angle of the angle is beta, and beta is not equal to 0.
Selecting a reference coordinate system, wherein the coordinate vector i, j, k of the reference system is determined according to the following formula:
Figure GDA0003515335220000071
under a reference coordinate system, the axis of rotation u0、u1Is shown as
Figure GDA0003515335220000072
Attitude is expressed in unit quaternion, interpolated linearly according to a spherical surface, by q0To q1The interpolation curve of (a) can be expressed by the following parametric equation:
q01(s)＝p01(s)q1，s∈[0，α0]
s is a parameter and
Figure GDA0003515335220000073
is represented by a gesture q1To a posture q01The rotational transformation of(s) is also expressed in terms of unit quaternion. p is a radical of01(s) and q1Operation betweenA quaternion multiplication is followed.
By spherical linear interpolation, from q1To q2The interpolation curve of (a) can be expressed by the following parametric equation:
q12(s)＝p12(s)q1，s∈[0，α1]
s is a parameter and
Figure GDA0003515335220000081
is represented by a gesture q1To a posture q12The rotational transformation of(s) is also expressed in terms of unit quaternion. p is a radical of12(s) and q1The operation between follows a quaternion multiplication.
To achieve a smooth transition in attitude, it is necessary to do so at q01(s) and q12(s) respectively selecting a starting point attitude qiAnd terminal attitude qfAnd a transition curve is constructed between the two attitudes.
Determining q from a given angle value alphaiAnd q isfAlpha satisfies alpha > 0, alpha < alpha0And alpha is less than alpha1. Will the gesture q1About an axis of rotation u0Rotation (alpha)0α) attitude of angular arrival as qiAttitude q1About an axis of rotation u1The attitude reached by rotating by an angle of alpha is taken as qfIs provided with
Figure GDA0003515335220000082
qiLocated on the interpolation curve q01(s) above, qfLocated on the interpolation curve q12(s) above.
Any posture can be changed from q1Obtained through a certain rotation transformation, and referenced to the interpolation curve q01(s) and q12In the form of(s), the parametric equation for the transition curve can be expressed in the form:
q(s)＝p(s)q1，s∈[0，σ] (2)
in the formula, s isThe parameter sigma is the maximum value of the parameter s, q(s) is any attitude on the transition curve, and p(s) is the attitude q1Rotation transformation to attitude q(s) expressed in unit quaternion, p(s) write component form
p(s)＝(p0(s)，(p1(s)，p2(s)，p3(s)))
If each component p0(s)，p1(s)，p2(s)，p3(s) are determined, as are p(s) and q(s).
p(s) each component is determined as follows, the last component p3(s) is constantly 0, the remaining three components have the following form:
Figure GDA0003515335220000083
in the formula, eta, thetamIs a constant related to alpha and beta, and the calculation formula is
Figure GDA0003515335220000091
Theta(s), f(s) is a function of s and is expressed as
Figure GDA0003515335220000092
Where sd, am, and Π are three special functions related to the elliptical integral: sd (u, m) corresponds to the Jacobi elliptic function sd (u | m), am (u, m) corresponds to the Jacobi amplitude function am (u | m), Π (n; φ, m) corresponds to the third type incomplete elliptic integral Π (n; φ | m), m is a parameter of the elliptic integral, and n is a feature number of the elliptic integral.
Further, in the formula (5), n is related to m, and n is expressed as an expression for m
Figure GDA0003515335220000093
cψRelated to m, n, the expression is
Figure GDA0003515335220000094
According to the formula θ (K (m)/c)ψ)＝θmDetermining m, i.e. solving equations
Figure GDA0003515335220000095
In the formula (8), K (m) is a first kind of complete elliptic integral, and pi (n; m) is a third kind of complete elliptic integral; from equations (7) and (6), equation (8) is bounded on both sides by m only, using numerical methods, e.g. dichotomy, in the interval [0, mmax]The equation for m determined by the above solution (8), wherein
Figure GDA0003515335220000096
After m is determined, n and c can be obtained from the formulas (6) and (7)ψThen, each component p(s) and(s) can be determined according to the formula (5) and the formula (3), finally, according to the formula (2), a parameter equation of the transition curve can be determined, and the maximum value sigma of the parameter s is determined by the following formula:
σ＝2K(m)/cψ (10)
p(s) determined by the above method can ensure that q (0) ═ qi，q(σ)＝qfI.e. the transition curve at the starting point and the previous attitude interpolation curve p01(s) joining, at the end point, with the subsequent attitude interpolation curve p12(s) joining, wherein the transition curve and the front and back interpolation curves have two-order geometric continuity at the joint; in addition, the derivative of the parameter in the transition curve parameter equation to the time is equal to the angular speed, so that the trajectory planning can be conveniently carried out according to the change rule of the angular speed.
The transition curve q(s) and the front-back attitude interpolation curve q constructed by the above method01(s) and q12(s) two-step geometric continuity at the junction, enabling angular velocityDegree smoothing and angular acceleration continuity; and the derivative of the parameter s of the transition curve q(s) to time is equal to the angular velocity, so that the trajectory planning is conveniently carried out according to the change rule of the angular velocity.
Correspondingly, the present embodiment further provides a system for smooth transition of posture, comprising
Determining the starting point attitude and the end point attitude of the transition curve, and knowing three attitudes q0，q1，q2Wherein by the attitude q0To q1Has a rotation axis of u0Angle of rotation alpha0Is greater than 0; from q1To q2Has a rotation axis of u1Angle of rotation alpha1＞0；u0And u1The included angle of the angle is beta, and beta is not equal to 0.
Selecting a reference coordinate system, wherein the coordinate vector i, j, k of the reference system is determined according to the following formula:
Figure GDA0003515335220000101
under a reference coordinate system, the axis of rotation u0、u1Is shown as
Figure GDA0003515335220000102
Attitude is expressed in unit quaternion, interpolated linearly according to a spherical surface, by q0To q1The interpolation curve of (a) can be expressed by the following parametric equation:
q01(s)＝p01(s)q1，s∈[0，α0]
s is a parameter and
Figure GDA0003515335220000103
is represented by a gesture q1To a posture q01The rotational transformation of(s) is also expressed in terms of unit quaternion. p is a radical of01(s) and q1The operation between follows a quaternion multiplication.
By spherical linear interpolation, from q1To q2The interpolation curve of (a) can be expressed by the following parametric equation:
q12(s)＝p12(s)q1，s∈[0，α1]
s is a parameter and
Figure GDA0003515335220000104
is represented by a gesture q1To a posture q12The rotational transformation of(s) is also expressed in terms of unit quaternion. p is a radical of12(s) and q1The operation between follows a quaternion multiplication.
To achieve a smooth transition in attitude, it is necessary to do so at q01(s) and q12(s) respectively selecting a starting point attitude qiAnd terminal attitude qfAnd a transition curve is constructed between the two attitudes.
Determining q from a given angle value alphaiAnd q isfAlpha satisfies alpha > 0, alpha < alpha0And alpha is less than alpha1. Will the gesture q1About an axis of rotation u0Rotation (alpha)0α) attitude of angular arrival as qiAttitude q1About an axis of rotation u1The attitude reached by rotating by an angle of alpha is taken as qfIs provided with
Figure GDA0003515335220000111
qiLocated on the interpolation curve q01(s) above, qfLocated on the interpolation curve q12(s) above.
The construction and the solving module of the parameter equation can lead any posture to be represented by q1Obtained through a certain rotation transformation, and referenced to the interpolation curve q01(s) and q12In the form of(s), the parametric equation for the transition curve can be expressed in the form:
q(s)＝p(s)q1，s∈[0，σ] (2)
wherein s is a parameter, σ is the maximum value of the parameter s, q(s) is any attitude on the transition curve, and p(s) is the attitude q1Rotation transformation to attitude q(s) expressed in unit quaternion, p(s) write component form
p(s)＝(p0(s)，(p1(s)，p2(s)，p3(s)))
If each component p1(s)，p2(s)，p3(s)，p4(s) are determined, as are p(s) and q(s).
p(s) each component is determined as follows, the last component p3(s) is constantly 0, the remaining three components have the following form:
Figure GDA0003515335220000112
in the formula, eta, thetamIs a constant related to alpha and beta, and the calculation formula is
Figure GDA0003515335220000113
Theta(s), f(s) is a function of s and is expressed as
Figure GDA0003515335220000114
Where sd, am, and Π are three special functions related to the elliptical integral: sd (u, m) corresponds to the Jacobi elliptic function sd (u | m), am (u, m) corresponds to the Jacobi amplitude function am (u | m), Π (n; φ, m) corresponds to the third type incomplete elliptic integral Π (n; φ | m), m is a parameter of the elliptic integral, and n is a feature number of the elliptic integral.
Further, in the formula (5), n is related to m, and n is expressed as an expression for m
Figure GDA0003515335220000121
cψRelated to m, n, the expression is
Figure GDA0003515335220000122
According to the formula θ (K (m)/c)ψ)＝θmDetermining m, i.e. solving equations
Figure GDA0003515335220000123
In the formula (8), K (m) is a first kind of complete elliptic integral, and pi (n; m) is a third kind of complete elliptic integral; from equations (7) and (6), equation (8) is only related to m on both sides, and the equation for m determined by equation (8) is solved over the interval [0, mmax ] using a numerical method, such as bisection, in which
Figure GDA0003515335220000124
After m is determined, n and c can be obtained from the equations (6) and (7)ψThen, each component p(s) and(s) can be determined according to the formula (5) and the formula (3), finally, according to the formula (2), a parameter equation of the transition curve can be determined, and the maximum value sigma of the parameter s is determined by the following formula:
σ＝2K(m)/cψ (10)
p(s) determined by the above method can ensure that q (0) ═ qi，q(σ)＝qfI.e. the transition curve at the starting point and the previous attitude interpolation curve p01(s) joining, at the end point, with the subsequent attitude interpolation curve p12(s) joining, wherein the transition curve and the front and back interpolation curves have two-order geometric continuity at the joint; in addition, the derivative of the parameter in the transition curve parameter equation to the time is equal to the angular speed, so that the trajectory planning can be conveniently carried out according to the change rule of the angular speed.
Example 2
This example provides that0＝120°，α1＝150°，α＝90°，β＝90°，q1＝(1，(0, 0, 0)), a detailed description of the attitude transition curve is determined.
From α and β, as obtained by equation (4)
Figure GDA0003515335220000125
θm＝arcsin(sin45°cos45°)＝30°
Obtained by the formula (6)
Figure GDA0003515335220000131
Obtained by the formula (9)
Figure GDA0003515335220000132
Using dichotomy, in the interval [0, mmax]Searching the root of m in the formula (8) to obtain
m＝0.161838
FIG. 1 shows the right side of the formula (8) in the interval [0, mmax]On the graph with ordinate of thetamThe abscissa value corresponding to the point of (a) is equal to m.
Calculated from the formula (6), the formula (7) and the formula (10)
n＝0.265482，cψ＝1.268094，σ＝2.587944
Mixing m, n, cψ，η，θmThe value of (3) is substituted in the order of formula (5) to obtain p0(s)，p1(s) and p2(s) detailed expressions; further, the expression of q(s) can be obtained from the formula (1), in this case, q1If it is (1, (0, 0, 0)), it can be known from quaternion multiplication rules
q(s)＝p(s)，s∈[0，σ]
I.e., q(s) is the same as the expression for p(s).
By spherical linear interpolation, from q0To q1Interpolation curve q of01(s) is
Figure GDA0003515335220000133
Figure GDA0003515335220000134
Also according to spherical linear interpolation, from q1To q2Interpolation curve q of12(s) is
Figure GDA0003515335220000135
Figure GDA0003515335220000136
q(s)，q01(s)，q12The three attitude curves represented by(s) lie on a three-dimensional unit sphere, and the last components of the three attitude curves are all constant to zero, and after the fourth component is ignored, q(s), q01(s)，q12(s) can be represented by a curve on a unit sphere in three-dimensional space, three attitude curves are illustrated in FIG. 2, and q(s) completes q01(s) to q12(s) smooth transition and at the junction point qiAnd q isfHere, two curves that meet have two-step geometric continuity.
Example 3
This embodiment provides an explanation of the trajectory planning of the attitude transition curve in embodiment 2 according to the change rule of angular velocity.
And (3) performing trajectory planning on the attitude transition curve obtained in the embodiment 2, namely determining a change rule s (t) of a parameter s in a transition curve parameter equation with respect to time. The known angular velocity change law is as follows: at the starting point qiAnd end point qfHas an angular velocity of 2 and is represented by qiTransition to qfThe angular velocity magnitude remains constant during the process.
By using the time derivative of the parameter s equal to the angular velocity, the time variation law s (t) of the parameter s can be obtained
s(t)＝2t，t∈[0，σ/2]
After q(s) and s (t) are determined, the change rule of the posture along with the time can be obtained, fig. 3 is a graph of the magnitude of the angular velocity ω and the change curve of each component along with the time, and the magnitude of the angular velocity | ω | is constantly 2. FIG. 4 is an angular acceleration
Figure GDA0003515335220000141
The magnitude and the time variation curve of each component, the magnitude of the angular acceleration of the starting point and the terminal point are both 0.
Example 4
Correspondingly, the present embodiment provides an attitude smooth transition processing apparatus, including at least one processor, and at least one memory communicatively connected to the processor, wherein: the memory stores program instructions executable by the processor to invoke the program instructions to perform the method of any of the embodiments described above.
Example 5
Accordingly, the present embodiments provide a computer-readable storage medium storing computer instructions that cause the computer to perform any of the above-described methods.
The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (8)

1. An attitude smooth transition method is characterized in that: the method comprises the following steps:
s01, according to given three postures q0，q1，q2Selecting a reference coordinate system, and determining a reference coordinate system by q according to a spherical linear interpolation method0To q1Attitude interpolation curve q of01(s) and q1To q2Attitude interpolation curve q of12(s); interpolating the curve q in the pose according to a given angle alpha01(s) selecting the starting attitude q of the transition curveiInterpolating the curve q in attitude12(s) selecting the terminal attitude q of the transition curvef；
S02, constructing and solving a parameter equation q(s) of a transition curve;
in step S02, the configuration of the transition curve and the parametric equation solving process are as follows: the parametric equation q(s) for the transition curve is first expressed in the form:
q(s)＝p(s)q1,s∈[0,σ] (2)
wherein s is a parameter of the transition curve, σ is a maximum value of the parameter s, q () is any attitude on the transition curve, and (q) is represented by q1The rotation transformation to any attitude q(s) on the transition curve is also expressed by unit quaternion, and the writing component is in the form of:
p(s)＝(p0(s),(p1(s),p2(s),p3(s)))
determining the components of p(s), wherein the last component p3(s) is constantly 0, and the remaining three components have the following formal expressions
Figure FDA0003548231960000011
In the formula (3), eta, thetamFor constants related to alpha and beta, the calculation formula is
Figure FDA0003548231960000012
Theta(s),(s) is a function of s and is expressed as
Figure FDA0003548231960000013
In equation (5), sd, am, and Π are three specific functions related to the elliptic integral: sd (u, m) corresponds to a Jacobi elliptic function sd (u | m), am (u, m) corresponds to a Jacobi amplitude function am (u | m), Π (n; φ, m) corresponds to a third type of incomplete elliptic integral Π (n; φ | m), m is a parameter of the elliptic integral, n is a characteristic number of the elliptic integral, and m has the following relation:
Figure FDA0003548231960000021
cψis a constant related to m, n and has the expression of
Figure FDA0003548231960000022
Solving for m again according to the formula θ (K (m)/cψ)＝θmSolving for m, i.e. solving equations
Figure FDA0003548231960000023
In the formula (8), K (m) is a first kind of complete elliptic integral, and pi (n; m) is a third kind of complete elliptic integral; from equations (7) and (6), equation (8) is bounded on both sides by m only, using a numerical method in the interval [0, m ]max]The equation for m determined by the above solution (8), wherein
Figure FDA0003548231960000024
Finally, a parameter equation q(s) of the transition curve is determined, and n and c can be solved according to the formula (6) and the formula (7) by the solved mψThen, each component p(s) and(s) can be determined according to the formula (5) and the formula (3), finally, according to the formula (2), a parameter equation of the transition curve can be determined, and the maximum value sigma of the parameter s is determined by the following formula:
σ＝2K(m)/cψ (10)
push type(3) The components of p(s) and p(s) determined in (8) can ensure the constructed transition curve q (S) and the front-back attitude interpolation curve q01(s)、q12(s) are each at qiAnd q isfSplicing, and having two-step geometric continuity at the splicing position; in addition, each component p(s) determined by the formulas (3) to (8) can also ensure that the derivative of the parameter s of the transition curve q(s) to time is equal to the angular velocity, so that the trajectory planning can be conveniently carried out according to the change rule of the angular velocity.
2. The pose smooth transition method according to claim 1, wherein in step S01, the reference coordinate system is selected as follows: remember u0，α0Is composed of q0To q1Rotation axis and rotation angle of u1，α1Is composed of q1To q2Rotation axis and rotation angle of u0And u1Is denoted as beta, the three coordinate vectors i, j, k of the reference coordinate system are determined by the following formula
Figure FDA0003548231960000025
From this, a reference coordinate system can be determined, in which the axis of rotation u0、u1Is shown as
Figure FDA0003548231960000031
3. The method for smooth transition of gesture according to claim 1, wherein in step S01, the gesture is expressed by unit quaternion according to the spherical linear interpolation method under the selected reference coordinate system, and the interpolation curve q is obtained01(s) and q12(s) can be represented as
Figure FDA0003548231960000032
Wherein
Figure FDA0003548231960000033
p01，p12Respectively represent by q1To q01(s) and the compound represented by the formula q1To q12(s) and the rotational transformation is also expressed by a unit quaternion.
4. The pose smooth transition method according to claim 1, wherein in step S01, a > 0 is satisfied for a given α, and α < α0And alpha is less than alpha1Attitude q of originiAnd terminal attitude qfIs selected according to the following formula
Figure FDA0003548231960000034
5. An attitude smoothing transition system, characterized by: comprises that
A transition curve starting point attitude and end point attitude determination module for determining three given attitudes q0，q1，q2Selecting a reference coordinate system, and determining a reference coordinate system by q according to a spherical linear interpolation method0To q1Attitude interpolation curve q of01(s) and q1To q2Attitude interpolation curve q of12(s); interpolating the curve q in the pose according to a given angle alpha01(s) selecting the starting attitude q of the transition curveiInterpolating the curve q in attitude12(s) selecting the terminal attitude q of the transition curvef；
A parameter equation constructing and solving module constructs and solves a parameter equation q(s) of the transition curve;
the construction of the transition curve and the calculation process of the parameter equation are as follows: the parametric equation q(s) for the transition curve is first expressed in the form:
q(s)＝p(s)q1,s∈[0,σ] (2)
wherein s is a parameter of the transition curve, σ is a maximum value of the parameter s, q(s) is any attitude on the transition curve, and p(s) is a composite of q1The rotation transformation to any attitude q(s) on the transition curve is also expressed by unit quaternion, and the writing component is in the form of:
p(s)＝(p0(s),(p1(s),p2(s),p3(s)))
determining the components of p(s), wherein the last component p3(s) is constantly 0, and the remaining three components have the following formal expressions
Figure FDA0003548231960000041
Eta, theta in the formula (3)mFor constants related to alpha and beta, the calculation formula is
Figure FDA0003548231960000042
θ(s), f(s) is a function of s and is expressed as
Figure FDA0003548231960000043
In equation (5), sd, am, and Π are three specific functions related to the elliptic integral: sd (u, m) corresponds to a Jacobi elliptic function sd (u | m), am (u, m) corresponds to a Jacobi amplitude function am (u | m), Π (n; φ, m) corresponds to a third type of incomplete elliptic integral Π (n; φ | m), m is a parameter of the elliptic integral, n is a characteristic number of the elliptic integral, and n and m have the following relations:
Figure FDA0003548231960000044
cψis a constant related to m, n and has the expression of
Figure FDA0003548231960000045
Solving for m again according to the formula θ (K (m)/cψ)＝θmSolving for m, i.e. solving equations
Figure FDA0003548231960000046
In the formula (8), K (m) is a first kind of complete elliptic integral, and pi (n; m) is a third kind of complete elliptic integral; from equations (7) and (6), equation (8) is bounded on both sides by m only, using a numerical method in the interval [0, m ]max]The equation for m determined by the above solution (8), wherein
Figure FDA0003548231960000051
Finally, a parameter equation q(s) of the transition curve is determined, and n and c can be solved according to the formula (6) and the formula (7) by the solved mψThen, each component p(s) and(s) can be determined according to the formula (5) and the formula (3), finally, according to the formula (2), a parameter equation of the transition curve can be determined, and the maximum value sigma of the parameter s is determined by the following formula:
σ＝2K(m)/cψ (10)
the components of p(s) determined according to the formulas (3) to (8) can ensure the constructed transition curve q(s) and the front-back attitude interpolation curve q01(s)、q12(s) are each at qiAnd q isfSplicing, and having two-step geometric continuity at the splicing position; in addition, each component p(s) determined by the formulas (3) to (8) can also ensure that the derivative of the parameter s of the transition curve q(s) to time is equal to the angular velocity, so that the trajectory planning can be conveniently carried out according to the change rule of the angular velocity.
6. An attitude smoothing transition system according to claim 5, wherein the attitude is at the start of the transition curveAnd in the terminal attitude determination module, the method for selecting the reference coordinate system comprises the following steps: remember u0，α0Is composed of q0To q1Rotation axis and rotation angle of u1，α1Is composed of q1To q2Rotation axis and rotation angle of u0And u1Is denoted as beta, the three coordinate vectors i, j, k of the reference coordinate system are determined by the following formula
Figure FDA0003548231960000052
From this, a reference coordinate system can be determined, in which the axis of rotation u0、u1Is shown as
Figure FDA0003548231960000053
7. An attitude smoothing transition system according to claim 6, wherein in the attitude determination module for the start point and the end point of the transition curve, the attitude is expressed by a unit quaternion according to a spherical linear interpolation method under the selected reference coordinate system, and the interpolation curve q is an interpolation curve q01(s) and q12(s) can be represented as
Figure FDA0003548231960000054
Wherein
Figure FDA0003548231960000055
p01，p12Respectively represent by q1To q01(s) and the compound represented by the formula q1To q12(s) and the rotational transformation is also expressed by a unit quaternion.
8. An attitude smoothing transition system according to claim 5, wherein in the transition curve start point attitude and end point attitude determination module, a given α satisfies α > 0, α < α0And alpha is less than alpha1Attitude q of originiAnd terminal attitude qfIs selected according to the following formula
Figure FDA0003548231960000061
CN202011225643.8A 2020-11-05 2020-11-05 Posture smooth transition method and system Active CN112356032B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011225643.8A CN112356032B (en) 2020-11-05 2020-11-05 Posture smooth transition method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011225643.8A CN112356032B (en) 2020-11-05 2020-11-05 Posture smooth transition method and system

Publications (2)

Publication Number Publication Date
CN112356032A CN112356032A (en) 2021-02-12
CN112356032B true CN112356032B (en) 2022-05-03

Family

ID=74510071

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011225643.8A Active CN112356032B (en) 2020-11-05 2020-11-05 Posture smooth transition method and system

Country Status (1)

Country Link
CN (1) CN112356032B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113103239B (en) * 2021-04-29 2022-09-13 哈尔滨工业大学 Robot attitude trajectory generation method and device and storage medium
CN113103240B (en) * 2021-04-29 2022-08-23 哈尔滨工业大学 Method, device and system for realizing C2 continuous robot trajectory planning

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2015199155A (en) * 2014-04-07 2015-11-12 キヤノン株式会社 Information processing device, information processing method, and program
CN105500354A (en) * 2016-02-02 2016-04-20 南京埃斯顿机器人工程有限公司 Transitional track planning method applied by industrial robot
CN108549322A (en) * 2018-04-11 2018-09-18 广州启帆工业机器人有限公司 Pose synchronization method and device for arc track motion of robot
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
CN111791236A (en) * 2020-07-17 2020-10-20 广东拓斯达科技股份有限公司 Industrial robot Cartesian space trajectory transition method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2015199155A (en) * 2014-04-07 2015-11-12 キヤノン株式会社 Information processing device, information processing method, and program
CN105500354A (en) * 2016-02-02 2016-04-20 南京埃斯顿机器人工程有限公司 Transitional track planning method applied by industrial robot
CN108549322A (en) * 2018-04-11 2018-09-18 广州启帆工业机器人有限公司 Pose synchronization method and device for arc track motion of robot
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
CN111791236A (en) * 2020-07-17 2020-10-20 广东拓斯达科技股份有限公司 Industrial robot Cartesian space trajectory transition method

Also Published As

Publication number Publication date
CN112356032A (en) 2021-02-12

Similar Documents

Publication Publication Date Title
CN105500354B (en) Transitional track planning method applied by industrial robot
CN111360827B (en) Visual servo switching control method and system
CN112356032B (en) Posture smooth transition method and system
JP2019517929A (en) Trajectory planning method of point-to-point movement in robot joint space
CN109834706B (en) Method and device for avoiding kinematic singularities in robot motion planning
CN109773780B (en) Pose synchronization method and device for transition path of mechanical arm
CN114378809B (en) Singular point free kinematic parameterization of soft robotic manipulators
CN109571481A (en) A kind of joint singular point processing method, device, equipment and storage medium
CN113650011B (en) Method and device for planning splicing path of mechanical arm
CN115179298A (en) Cartesian space trajectory planning method and device
CN116125906A (en) Motion planning method, device and equipment for numerical control machining and storage medium
CN113618741B (en) Robot joint pose optimization method, robot control method and robot
CN105563482A (en) Rotation movement planning method for end effector of industrial robot
CN113276116B (en) Error-controllable robot track synchronous transition method
Park et al. Tracking on lie group for robot manipulators
Gubankov et al. Comparison study of different types of setting reference movements for mechatronic objects by parametric splines
CN107807612B (en) Numerical control machine tool spherical surface machining method based on quaternion spiral spherical surface interpolation method
JP4667794B2 (en) Numerical control method, numerical control device, program, and computer-readable recording medium
JP4667796B2 (en) Numerical control method, numerical control device, program, and computer-readable recording medium
Shahbazi et al. On the orientation planning with constrained angular velocity and acceleration at endpoints
CN117407630B (en) Inverse kinematics analysis and solution system for solving shoulder and elbow angles through cooperative mechanical arm
CN113103241B (en) Method, device and system for realizing G2 continuous robot double-NURBS track interpolation
KR102520793B1 (en) Method for blending motion route of robot and apparatus thereof
CN113305474B (en) Welding robot welding attitude interpolation method based on PLC
Steuben et al. NURBS for robot manipulator trajectory generation

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