JP2005276156A - Numerical control method and device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide the definition formula of a new three-dimensional clothoid curve succeeding to the characteristics of a two-dimensional clothoid curve in which a curvature change pattern for independent variables is simple for executing the numerical control of the motion of a tool. <P>SOLUTION: This numerical control method is provided to express the route of a tool or the profile of a work by using a three-dimensional curve_(called three-dimensional clothoid curve) in which each of a pitch angle and yaw angle in tangential direction is given by the quadratic of curve length or curve length variables, and to control the motion of a tool by the three-dimensional curve. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a numerical control method and apparatus for controlling the movement of a tool (including a gripping part such as a hand and various tools) in a work machine (referred to as a robot) such as a robot, a machine tool, an assembly machine, and an inspection machine. In particular, the present invention relates to a numerical control method and apparatus for controlling a three-dimensional movement of a tool.

  In a robot that performs numerical control such as welding, painting, and adhesive application, an input figure is generally input as discrete point sequence data. Therefore, in order to generate a continuous figure, it is necessary to interpolate the point sequence using some method.

  As methods for interpolating between arbitrarily given point sequences, methods of rounding the corners of broken lines, B-spline interpolation, cubic spline interpolation, etc. are known, but they can pass through each given point strictly. As an interpolation method, cubic spline interpolation is known (see Non-Patent Document 1).

  However, since cubic spline interpolation is expressed as an independent variable that has no geometric meaning, the relationship between the independent variable and the geometric quantities of the curve is indefinite. have. This cubic spline interpolation has a complicated relationship between the moving distance from the starting point and the curvature, and is not suitable for control that keeps the linear velocity constant.

  In two dimensions, clothoid interpolation is proposed by the inventors as an interpolation method that passes through each given point, and it is known that smooth interpolation can be performed (see Non-Patent Document 2). Therefore, if the clothoid curve is extended to three dimensions and used for interpolation of a free point sequence, the linear velocity can be kept constant or changed according to the line length due to the characteristics of the clothoid curve expressed as a function of the curve length. It seems that such control can be easily realized. Moreover, since the curve length is used as a parameter, there is an advantage that the line length does not need to be obtained later, unlike other methods. It is useful to extend clothoid interpolation to three dimensions in fields such as numerical control. Be expected. Lith et al.'S "3D Discrete Clothoid Splines" (see Non-Patent Document 3) is known as an extension of clothoid curves to three dimensions, but the clothoid curves have been extended to three dimensions in the form of equations. What you did is not known. The expansion in the form of an expression is advantageous in that each value can be easily calculated.

  Therefore, the present invention provides a new three-dimensional clothoid curve definition formula in which the curvature change pattern with respect to the independent variable inherits the characteristics of a simple two-dimensional clothoid curve as much as possible in order to numerically control the movement of the tool. The purpose is to interpolate a sequence of points with a curve.

Hosaka Mamoru and Sada Toshio, "Integrated CAD / CAM System", Ohmsha, 1997 Aoi Tome, Hiroshi Makino, Daiharu Suda, Yasuo Yokoyama, "Free Curve Interpolation Method Using Clothoid" (Journal of Robotics Society of Japan, Vol. 8, No. 6, pp40-47) Li Guiqing, Li Xianmin, Li Hua "3D Discrete Clothoid Splines", (CGI’01, pp321-324)

  The present invention will be described below.

  The invention according to claim 1 is a three-dimensional clothoid curve in which a pitch angle and a yaw angle in a tangential direction are each given by a quadratic expression of a curve length or a curve length variable between point sequences arbitrarily given in three-dimensional coordinates. The above-described problems are solved by a numerical control method that interpolates using (three-dimensional clothoid segment) and controls the movement of the tool using the three-dimensional clothoid segment.

  The invention of claim 2 connects a plurality of three-dimensional clothoid curves (three-dimensional clothoid segments) in which the pitch angle and the yaw angle in the tangential direction are each given by a quadratic expression of a curve length or a curve length variable. This is a numerical control method for controlling the movement of the tool by the three-dimensional clothoid segment.

  According to a third aspect of the present invention, in the numerical control method according to the first or second aspect, a three-dimensional clothoid curve is defined by the following equation.

はそれぞれ、３次元クロソイド曲線上の点の位置ベクトル、及びその初期値を示す。 here,

Indicates the position vector of the point on the three-dimensional clothoid curve and its initial value.

Let the length of the curve from the start point be s, and the total length (the length from the start point to the end point) be h. The value obtained by dividing s by h is represented by S. S is a dimensionless value and is called a curve length variable.
i, j, and k are unit vectors in the x-axis, y-axis, and z-axis directions, respectively.
u is a unit vector indicating the tangent direction of the curve at point P, and is given by equation (2). Ekβ and Ejα are rotation matrices representing the rotation of the angle β around the k axis and the rotation of the angle α around the j axis, respectively. The former is called yaw rotation, and the latter is called pitch rotation. Equation (2) shows that the tangent vector u is obtained by first rotating the unit vector in the i-axis direction by α around the j-axis and then by β around the k-axis.
a ₀ , a ₁ , a ₂ , b ₀ , b ₁ , b ₂ are constants.

According to a fourth aspect of the present invention, in the numerical control method according to the third aspect, in the joint of one three-dimensional clothoid segment and the next three-dimensional clothoid segment, the position and tangential direction (and curvature in some cases) of both are continuous. As described above, seven parameters a ₀ , a ₁ , a ₂ , b ₀ , b ₁ , b _2, h of the three-dimensional clothoid segment are calculated. The position and tangential direction are always continuous, and the curvature may be continuous as necessary.

  The invention according to claim 5 is a three-dimensional clothoid segment in which a pitch angle and a yaw angle in a tangential direction are each given by a quadratic expression of a curve length or a curve length variable between point sequences arbitrarily given in three-dimensional coordinates. Is a numerical control device that controls the movement of the tool by this three-dimensional clothoid segment.

  According to the sixth aspect of the present invention, in order to numerically control the movement of the tool, the tangential pitch angle and yaw angle are respectively set to a curve length or a curve between points arbitrarily given in three-dimensional coordinates. This is a program for functioning as a means for interpolation using a three-dimensional clothoid segment given by a quadratic expression of a long variable.

  According to the seventh aspect of the present invention, in order to numerically control the movement of the tool, the computer is arranged between a sequence of points arbitrarily given in three-dimensional coordinates, and each of the tangential pitch angle and yaw angle is a curve length or a curve. A computer-readable recording medium on which a program for functioning as means for interpolating using a three-dimensional clothoid segment given by a quadratic expression of a long variable or a calculation result by the program is recorded.

  According to the present invention, the main variable of the three-dimensional clothoid segment is the curve length or the curve length variable, and the pitch angle and yaw angle in the tangential direction are given by the quadratic expression of the curve length or the curve length variable, respectively. It is guaranteed that the normal direction obtained by differentiating the dimensional clothoid segment once and the curvature obtained by differentiating twice are continuous with respect to the curve length or the curve length variable. In other words, the normal direction and the curvature are continuous in one clothoid segment. Accordingly, a smooth and good clothoid segment can be obtained, and a numerical control system that realizes a speed change that is mechanically reasonable is possible.

  Hereinafter, the present invention will be described in order by defining and characterizing a three-dimensional clothoid curve, and an interpolation method using a three-dimensional clothoid curve and a numerical control method using three-dimensional clothoid interpolation.

1. Definition and Features of 3D Clothoid Curve (1) Basic Formula of 3D Clothoid Clothoid curve, also known as Cornu's spiral, is a curve whose curvature changes in proportion to the length of the curve. It is.
A conventionally known two-dimensional clothoid curve is a kind of plane curve (two-dimensional curve), and is represented by the following expression on the xy coordinates shown in FIG.

  here,

Is a position vector representing a point on the curve,

Is the initial value (start point position vector).

Is a unit vector (vector with a length of 1) representing the tangential direction of the curve, and its direction φ is measured counterclockwise from the original line (x-axis direction). When this unit vector is integrated over a minute length ds, a point P on the curve is obtained.

  The length from the start point of the curve measured along the curve is s, and the total length (the length from the start point to the end point) is h. A value obtained by dividing s by h is represented by S. S is a dimensionless value and is called a curve length variable.

The characteristic of the clothoid curve is that the tangential direction angle φ is expressed by a quadratic expression of the curve length s or the curve length variable S as shown in the equation (2). c ₀ , c ₁ , c ₂ or φ ₀ , φ _v , φ _u are quadratic coefficients, and the total length h of these curves is called a clothoid parameter. FIG. 2 shows the shape of a general clothoid curve.

  The above relationship is expanded to three dimensions to create a three-dimensional clothoid curve equation. Conventionally, the formula giving a three-dimensional clothoid curve has not been known, but the inventors have derived this for the first time.

  A three-dimensional clothoid curve is defined by the following equation.

here,

Indicates the position vector of the point on the three-dimensional clothoid and its initial value. i, j, and k are unit vectors in the x-axis, y-axis, and z-axis directions, respectively.

u is a unit vector indicating the tangent direction of the curve at point P, and is given by equation (7). In Equation (7), E ^kβ and E ^jα are rotation matrices, and as shown in FIG. 3, the rotation of the angle β around the k axis (z axis) and the angle α around the j axis (y axis), respectively. Represents the rotation. The former is called yaw rotation, and the latter is called pitch rotation. Equation (7) is expressed as i-axis (
It is shown that the tangent vector u can be obtained by first rotating the unit vector in the x-axis direction by α around the j-axis (y-axis) and then by β around the k-axis (z-axis). .

That is, in the two-dimensional case, the unit vector ^ejφ representing the tangential direction of the curve is obtained from the inclination angle φ from the x axis. In the three-dimensional case, the tangent vector u of the curve can be obtained from the pitch angle α and the yaw angle β. When the pitch angle α is 0, a two-dimensional clothoid curve wound in the xy plane is obtained, and when the yaw angle β is 0, a two-dimensional clothoid curve wound in the xz plane is obtained. A three-dimensional clothoid curve is obtained by integrating the tangential vector u with a minute length ds.

  In the three-dimensional clothoid curve, the pitch angle α and the yaw angle β of the tangent vector are given by the quadratic expression of the curve length variable S as shown in the equations (8) and (9), respectively. This makes it possible to freely select a change in the tangential direction and to give the change continuity.

  As shown by the above formula, the three-dimensional clothoid curve is defined as “the tangential direction pitch angle and yaw angle are curves each represented by a quadratic expression of a curve length variable”.

One 3D clothoid segment starting from P ₀ is

Determined by the seven parameters. The six variables a ₀ to b ₂ have angular units and represent the shape of the clothoid segment. In contrast, h has a unit of length and represents the size of the clothoid segment.

  A typical example of a three-dimensional clothoid curve is a spiral curve as shown in FIG.

(2) In the moving frame structure (7), substituting the basic coordinate system [i, j, k] for the basic tangent direction vector i, the following moving frame frame E is obtained.

Here, v and w are unit vectors included in a plane perpendicular to the tangent of the curve, and are orthogonal to each other and orthogonal to the tangential unit vector u. The set of three unit vectors (triad) is a frame (coordinate system, frame) that moves with the moving point P, and this is called a moving frame.

  Since the moving frame is obtained by the above formula, the calculation of the main normal and the sub normal is facilitated, and the shape analysis of the curve can be easily performed.

  Further, the posture of the tool point of the robot can be obtained using E, and the position and posture of the object held by the robot hand can be obtained.

If the initial value and the closing price of E are E ₀ and E ₁ respectively,

It becomes.

(3) Rolling By considering the moving frame, the third rotation “roll” can be handled.
The roll is a tangential rotation. The presence of the roll does not affect the shape of the 3D clothoid itself, but does affect the moving frame structure induced by the 3D clothoid. An abacus ball passed through a winding wire can freely rotate around the wire, but that does not change the shape of the wire.

  When considering roll rotation, the moving frame structure is as follows.

 The roll angle γ can also be expressed as a function of S.

(4) Geometric properties of 3D clothoid curves
(a) Normal of 3D clothoid curve
It is known that a normal vector of a three-dimensional curve is expressed by the following equation using a tangential direction vector u.

  Here, from equation (7), the first derivative of the tangent vector of the three-dimensional clothoid curve is as follows.

  That is, the normal vector of the three-dimensional clothoid curve is expressed in the following form using S.

(b) Normal of a three-dimensional clothoid curve using rotation Here, consider the normal n as well as the determination of the tangent u in (7). Assume that the initial normal direction is represented by (0, cos γ, −sin γ) using a constant γ with respect to the initial tangential direction (1,0, 0). When this is rotated in the same way as the tangent line, the normal n is expressed as follows.

  (20) Comparing the equations of (21), it can be seen that sin γ and cos γ correspond to the following.

(c) Normal continuity at the connection point in 3D clothoid interpolation To achieve normal continuity at the connection point in 3D clothoid interpolation, from equation (22),

However, it turns out that it should just be continuous.

(D) Curvature of 3D clothoid curve
The curvature of the three-dimensional clothoid curve is expressed by the following equation.

  From equation (19), the curvature is

It is expressed.

(5) Features of 3D clothoid curve
(a) Continuity of curve In one clothoid segment (a clothoid represented by the same parameter), the pitch angle and yaw angle in the tangential direction are given by the quadratic expression of the curve length variable S. It is guaranteed that the normal direction obtained by the second differentiation and the curvature obtained by the second differentiation are continuous with respect to the curve length variable S. In other words, the normal direction and the curvature are continuous in one clothoid segment. Therefore, a smooth and good characteristic curve can be obtained. Also, when two clothoid curves are connected, a smooth single curve can be created by selecting parameters so that the tangent, normal, and curvature are continuous at the joint. This is called clothoid spline.

(b) Applicability Since the tangential direction of the curve can be swung by two angles (pitch angle and yaw angle), a three-dimensional curve can be arbitrarily created according to various conditions and used for various applications. Can do.

(c) Consistency with geometric curves Geometric curves such as straight lines, arcs, and screw curves can be created by setting some of the clothoid parameters to zero, or by giving a specific functional relationship between some parameters. . These curves are a kind of clothoid curve and can be expressed using a clothoid format. Therefore, unlike the conventional NC, it is not necessary to change the format described by straight lines, arcs, free curves, etc., and calculation and control can be performed using the same format.

In addition, by always setting either α or β to 0, a two-dimensional clothoid can be created, so that resources already obtained for the two-dimensional clothoid can be utilized.

  That is, individual curves such as arcs and straight lines including the already known two-dimensional clothoid can be expressed by appropriately setting α and β. Since the same three-dimensional clothoid curve formula can be used for such individual curves, the calculation procedure can be simplified.

(d) Good visibility With conventional interpolation methods such as spline interpolation, the overall shape or local shape of a free curve is often difficult to understand, but in 3D clothoids, the pitch By assuming each of the angle and the yaw angle, the entire image can be grasped relatively easily.

  Further, as soon as it is expressed as a clothoid curve, values such as line length, tangential direction, curvature, etc. are already known, and there is no need to recalculate like the conventional interpolation method. That is, corresponding to the parameter S of the curve, the tangent, normal, and curvature of the curve can be directly obtained as shown in equations (7), (20), and (26). This is a very effective feature for the numerical control method described later. As a result, the calculation time can be greatly shortened, resources such as memory can be saved, and real-time interpolation can be performed.

  In NC machining, the minimum curvature radius of the tool trajectory is an important problem, and spline interpolation and the like require complicated calculations. However, in clothoids, the value of the minimum curvature radius is generally known for each segment. This is advantageous in selecting the cutter diameter.

(e) Ease of motion control The main variable of the curve is the length or the normalized length S, and the equation of the curve is given by the natural equation for this length. For this reason, by defining the length s as a function of the time t, motion characteristics such as acceleration / deceleration can be arbitrarily given, and by adopting a motion curve with good characteristics that has been used in conventional cams and the like, Work speed can be increased. Since the length s is given as a value in the actual Cartesian space, and the velocity / acceleration is obtained with respect to the tangential direction, it is not necessary to synthesize the given values for each axis as in the conventional interpolation method. Further, since the curvature can be easily calculated, the centrifugal acceleration during the exercise can be easily obtained, and the control according to the motion trajectory can be performed.
(6) Generation of curve and property of each parameter According to the definition, the influence of each parameter of the three-dimensional clothoid curve on the curve is as follows. By giving each parameter, a three-dimensional clothoid curve can be generated as shown in FIG.
Table 1 summarizes the properties of each parameter of the three-dimensional clothoid curve.

2. Interpolation method using three-dimensional clothoid curve (1) Mathematical condition for smooth connection A single three-dimensional clothoid curve has a limit in the expression of the shape of the curve. Here, a plurality of three-dimensional clothoid curves (three-dimensional clothoid segments) are connected for the purpose of controlling the movement of the tool by numerical control, and the movement of the tool is controlled by the plurality of three-dimensional clothoid segments.

  The smooth connection of two three-dimensional clothoid curves at their end points is defined as the end point position, tangent and curvature being connected continuously. Using the above definition, this condition is described as follows: The first three formulas represent position continuity, the next two formulas represent tangent continuity, the next one represents normal coincidence, and the last formula represents curvature continuity.

  This is a sufficient condition for tangent and normal vectors, curvature and α, β continuity at the connection point, and the condition may be too tight. Therefore, the conditions can be changed as follows so as to satisfy the conditions purely.

  Further here

Considering that

Is replaced by the following conditions:

  It turns out that the objective can be achieved if the following conditions are satisfied.

In equation (28), the first three equations indicate position continuity, the next two equations represent tangential continuity, the next equation represents normal direction coincidence, and the last equation represents curvature continuity. . To do G ² continuous interpolation, it is necessary to three-dimensional clothoid curve two meet seven conditional expression (28) at its end points.

G ² continuous (G is Geometry initials of) supplement for. Figure 5 shows the condition of G ² consecutive interpolation.

G ⁰ means that the three-dimensional clothoid curve two coincide is located at its end points and continuous refers to the tangential direction coincides with G ¹ continuous, and G ² continuous contact plane (normal) and This means that the curvatures match. The following table compares the C ^{0 to} C ² continuity used in the spline curve and the G ^{0 to} G ² continuation used in the clothoid curve of the present invention.

When considering the continuity of two three-dimensional clothoid curves, the interpolation conditions become more severe as C ⁰ → C ¹ → C ² and G ⁰ → G ¹ → G ² . For C ¹ continuation, the size and direction of the tangent need to match, but for G ¹ continuation, only the tangential direction needs to match. If smoothly connecting tangents with 3-dimensional clothoid curve two has better to create a condition in G ¹ continuous. If a conditional expression is created with C ¹ continuation like a spline curve, a condition that the sizes of tangents which are not geometrically related coincide with each other, so that the condition becomes too strict. Creating a conditional expression with G ¹ continuous has the advantage that the magnitude of the primary differential coefficient can be freely set.

G ² is the continuous match contact plane (normal). The contact plane means planes S1 and S2 in which the curve C is locally included as shown in FIG. FIG. 6 shows an example in which the tangential direction is continuous at the point P but the contact planes S1 and S2 are discontinuous. When considering the continuity of the three-dimensional curve, what must be considered next to the coincidence in the tangential direction is the coincidence of the contact planes. When discussing the curvature, it is meaningless if the contact planes do not match, and it is necessary to match the curvatures after matching the contact planes. In 3-dimensional curve of the two coordinates, tangential, contact plane (normal direction) and to match the curvature becomes a condition satisfying G ² continuous.

(2) Specific calculation procedures There are the following two types of calculation procedures.

(a) A curve parameter h, α, β is given to generate one three-dimensional clothoid curve, and the parameters of the next three-dimensional clothoid curve are determined so as to satisfy the equation (28) at the end point. In this way, it is possible to generate a three-dimensional clothoid curve that connects smoothly one after another. According to this calculation procedure, calculation of curve parameters is easy, and this is called a forward solution. According to this method, curves having various shapes can be easily generated, but the connection point through which the curve passes cannot be explicitly specified.

(b) A three-dimensional clothoid curve can be connected so that a point group designated in advance becomes a connection point of the curve. Here, a short clothoid curve (clothoid segment) is created for each section of a point sequence arbitrarily given discretely. In this case, the calculation procedure for determining the curve parameters so as to satisfy the equation (28) is more complicated than (a), and is repeated convergence calculation. This calculation procedure is called an inverse solution because the curve parameter is determined in reverse from the connection conditions.

The calculation method is described in detail for the inverse solution of (b) above. The calculation problem to be solved is formulated as follows.

Unknown parameter: curve parameter Constraint condition: Equation (28) or a part thereof Depending on the required problem, the number of constraint conditions changes, and the number of curve parameters corresponding to the constraint condition may be set as unknown parameters. For example, when curvature continuity is not required, some curve parameters can be moved freely. Alternatively, when the curvature is continuous and the tangential direction is specified, it is necessary to increase the number of corresponding unknown curve parameters by increasing the number of three-dimensional clothoid curves used for interpolation by division.

  In order to make the above-mentioned repeated convergence calculation converge stably, a device for calculation is required. In order to avoid divergence of calculation and speed up convergence, it is effective to set better initial values for unknown parameters. For this purpose, a simpler interpolation curve, such as a linear spline curve, that satisfies the constraint conditions of a given connection point, etc. is generated, and the curve parameters of the three-dimensional clothoid curve are estimated from the curve shape to repeatedly calculate convergence. It is effective to set the initial value of.

  Alternatively, a method of sequentially increasing the conditional expressions instead of satisfying the constraint conditions to be satisfied at once is also an effective method for obtaining a stable solution. For example, the curve generation procedure is divided into the following three STEPs and executed sequentially. Interpolation is performed so that the position information and the tangential direction coincide with each other as the first STEP, and then interpolation is performed so that the normal direction coincides with the second STEP, and interpolation is performed so that the curvature also coincides with the third STEP. An outline of the flow of this method is shown in FIG. The necessary three-dimensional clothoid curve formula and its tangent, normal and curvature definition formulas have already been shown.

(3) Example of interpolation method using 3D clothoid curve
(a) Flow of interpolation method An embodiment of a method for smoothly interpolating between given point sequences using a three-dimensional clothoid curve will be described in detail. The interpolation method using a three-dimensional clothoid curve is hereinafter referred to as three-dimensional clothoid interpolation. The entire curve group generated by the interpolation is called a three-dimensional clothoid curve, and the unit curve constituting the group is called a three-dimensional clothoid segment.

The basic flow of three-dimensional clothoid interpolation is to make each parameter of the three-dimensional clothoid segment connecting the points to be interpolated unknowns, satisfy the conditions that strictly pass through the points to be interpolated and are G ² continuous. Is generated by the Newton-Raphson method to generate a curve. A summary of this flow is shown in FIG. Here, G ² continuity means that the positions, tangential directions, normal directions, and curvatures of two three-dimensional clothoid curves coincide at the end points.

(B) ^{G 2} consecutive interpolation conditions
In three-dimensional clothoid interpolation, specific conditions are considered for conditions that pass strictly through the points to be interpolated and are G ² continuous.

Now there are simply three points P ₁ = {Px ₁ , Py ₁ , Pz ₁ }, P ₂ = {Px ₂ , Py ₂ , Pz ₂ }, P ₃ = {Px ₃ , Py ₃ , Pz ₃ } Consider interpolating the point with a 3D clothoid segment. FIG. 9 shows three-dimensional clothoid interpolation of points P ₁ , P ₂ and P ₃ . Point P _1, P ₂ between curve C ₁ and curve connecting, the curve connecting the points P _2, P ₃ and curve C _2, unknowns in this case, the parameter a0 ₁ of the curve C _1, a1 _1, a2 ₁ , b0 ₁ , b1 ₁ , b2 ₁ , h ₁ and 14 parameters a0 ₂ , a1 ₂ , a2 ₂ , b0 ₂ , b1 ₂ , b2 ₂ , h ₂ of the curve C ₂ . The character subscripts described below correspond to the subscripts of each curve.

Here, let us consider a condition that strictly passes through the point to be interpolated and is G ² continuous. First, considering the definition of a three-dimensional clothoid curve, the condition that the starting point strictly passes through the interpolation target point is inevitably achieved when the starting point is given, so there is no interpolation condition. Next, at the connection point P ₁ , three expressions for the position, two for the tangent vector, and two expressions for the continuity of curvature, two in terms of magnitude and direction, hold. As for the end point, the point P ₂ has three positions. From the above, there are 10 conditional expressions in total. However, since there are only 10 conditional expressions for 14 unknowns, it is not possible to obtain an unknown solution. Therefore, in this study, tangent vectors of both end points are given, and two conditions are increased for both end points to make the number of conditional expressions equal to the number of unknowns. In addition, if the tangent direction at the starting point is determined, a0 ₁ and b0 ₁ can be obtained from their defining formulas, so they are not treated as unknowns. Each condition will be considered below.

  First, considering the position conditions, the following three are established from the equations (1-1), (1-2), and (1-3). (Hereafter, natural number i <3)

  Next, considering the tangential direction, the following two expressions (1-4) and (1-5) hold.

  As for the magnitude of the curvature κ, the following equation (1-6) is established.

  Finally, consider the normal direction vector n. The normal vector n of the three-dimensional clothoid curve is expressed by equation (21).

Here, let us consider the normal vector n using rotation in the same manner as the determination of the tangent vector u of the three-dimensional clothoid curve. Assume that the initial normal direction is expressed by (0, cos γ, −sin γ) using a constant γ with respect to the initial tangent direction (1,0, 0). If this is rotated in the same way as the tangent, the normal n is given by the equation (1-7)
It is expressed as

(21) Comparing equations (1-7), it can be seen that sinγ and cosγ correspond to equation (1-8).

That is, it can be seen from Equation (1-8) that tan γ needs to be continuous in order to achieve normal continuity at the connection point in three-dimensional clothoid interpolation.

That is, it can be seen that the condition of normal continuity is the expression (1-10).

  Further here

Taking this into consideration, the conditional expression (1-10) is replaced by the following conditional expression (1-12). That is, the condition of normal continuity is the expression (1-12).

Summarizing the above, it can be seen that the condition that exactly passes through the points to be interpolated and is G ² continuous is as shown in Equation (1-13) at the connection point. Also, some of these conditions are selected at the start and end points.

From the above, for 12 unknowns a1 ₁ , a2 ₁ , b1 ₁ , b2 ₁ , h ₁ , a0 ₂ , a1 ₂ , a2 ₂ , b0 ₂ , b1 ₂ , b2 ₂ , h ₂ , the conditional expression is It can be seen that 12 of these hold. (The tangential rotation angles at the point P ₃ are α ₃ and β ₃ )

  Since 12 equations are established for 12 unknowns, a solution can be obtained. Solve this by the Newton-Raphson method to find the solution.

In general, when considering interpolation of n number of point sequences, the conditional expression may be expanded from the natural number i described above to i <n. The remaining problem is the number of unknowns and conditional expressions.

For example, when there are n−1 point sequences, N unknowns and N relational expressions hold. If the number further increases by one point, the number of unknowns increases by seven of the clothoid parameters a0 _n , a1 _n , a2 _n , b0 _n , b1 _n , b2 _n , and h _n of the three-dimensional clothoid segments P _n−1 and P _n . On the other hand, since the number of connection points increases by one, the conditional expression has three points for the position at the point P _n-1 , two for the tangent vector, and two for the continuity of curvature at the point P _{n-1 for} the size and direction. Increase by 7.

  Since it is known that there are 12 unknowns and relational expressions when n = 3, when n ≧ 3, 7 (n−2) +5 unknowns and 7 (n−2) +5 There are pieces. Since the unknown and the number of conditions related thereto are equalized, it is possible to obtain a solution in the same manner as in the case of n free point sequences with 3 free points. The solution was solved using the Newton-Raphson method, which utilizes the fact that the relationship of (1-15) and (1-16) holds between the unknown and the conditional expression. (Conditions are F unknown u and error Jacobian matrix J.)

From the above, it can be seen that n-dimensional sequence interpolation can be performed so that n point sequences strictly pass through the interpolation target points and are G ² continuous.

(c) Determination of initial value In the Newton-Raphson method, it is necessary to give an appropriate initial value when starting the search for a solution. The initial value may be given in any way, but here, an example of how to give the initial value will be described.

  3D Discrete Clothoid Splines, which is a previous research, has the property that the curvature changes smoothly with respect to the moving distance from the starting point, strictly passing through the interpolation target point. Therefore, in this study, the initial value for 3D clothoid interpolation was determined by calculating the polygon Q of 3D Discrete Clothoid Splines with r = 4 as shown in Fig. 10 and calculating from that.

Here is a supplementary explanation of 3D Discrete Clothoid Splines. As shown in FIG. 11, first, a polygon P having vertices as a sequence of points to be interpolated is created, and new vertices are inserted between the vertices of P by the same number r so that P 多 Q is obtained. Make a square Q. Here, assuming that there are n vertices of P, the polygon Q has rn vertices when closed, and r (n-1) +1 vertices when open. Hereinafter, each vertex is represented by qi with the subscript as a serial number from the starting point. At each vertex, a vector k having a normal vector b as a direction and a curvature κ as a magnitude is determined.

  At this time, the following formula (1-17) is satisfied so that the following vertices are equidistant, and when the curvature is closest to the condition in which the curvature is proportional to the moving distance from the start point (of formula (1-18) Polygon Q (when minimizing the function) is called 3D Discrete Clothoid Splines.

In 3D Discrete Clothoid Splines, the Frene frame of each vertex has already been obtained. Therefore, parameters a ₀ and b ₀ are obtained from the unit tangent direction vector t. This tangential direction vector t is already known when the polygon Q is obtained, and the tangential direction rotation angles α and β of the apex of the polygon Q are obtained from this t and the tangent equation of the three-dimensional clothoid curve. Thereby, initial values of a ₀ and b ₀ of each curve are obtained. The value is given for the 3D clothoid line segment starting from the start point.

Here, 3D Discrete Clothoid Splines can be approximated as having a curve length variable S of 1/4 at a point q _{4i + 1} in FIG. 10, considering that the vertices are arranged at equal distances. Similarly, at the point q _{4 (i + 1) −1} , it can be approximated that the curve length variable S is 3/4. Considering these together with the α equation of the three-dimensional clothoid curve, the following equation (1-20) is established.

This equation is a two-dimensional simultaneous equation with unknown _numbers a1 _4i and a2 _4i , which are solved and set as initial values of parameters a ₁ and a ₂ . Similarly, the initial values of the parameters b ₁ and b ₂ can be determined.

  The remaining unknown is the curve length h. The initial value is calculated from the curvature equation of the three-dimensional clothoid curve. The curvature of the three-dimensional clothoid curve is expressed by equation (1-21).

  When this equation is transformed, it becomes the equation (1-22), and the initial value of h is determined.

With the above method, initial values can be determined for seven three-dimensional clothoid parameters. The approximate value of the parameter of each curve under conditions such that G ² consecutive as described in reference to the determined initial value (b) were determined by the Newton-Raphson method. A three-dimensional clothoid line segment was generated from the obtained parameters, and the point sequence was interpolated with a three-dimensional clothoid curve.

(d) Interpolation example As an example of interpolating the point sequence by the method described above, (0.0, 0.0, 0.0), (2.0, 2.0, 2.0), (4.0, 0.0, 1.0), (5.0, 0.0, 2.0 ) Is a three-dimensional clothoid interpolation example. A perspective view of the three-dimensional clothoid curve generated by the interpolation is shown in FIG. In FIG. 12, the solid line is a three-dimensional clothoid curve, and the broken line, the alternate long and short dash line, the magnitude of each point on the curve is log (curvature radius + natural logarithm e), and the direction is a normal vector. It is a curvature radius change pattern.

Table 3 shows the parameters of each curve, and Table 4 shows the deviation of coordinates, tangents, normals, and curvatures at each connection point. It can be seen that three-dimensional clothoid curve such that G ² consecutive each from these connection points are generated. FIG. 13 is a curvature change graph in which the horizontal axis represents the movement distance from the starting point and the vertical axis represents the curvature.

(4) G ² continuous 3D clothoid interpolation considering the control of each value at both ends
(a) Interpolation conditions and unknowns As described in (3) above, when a curve is open and there are n points to be interpolated, the point sequence is interpolated with n-1 curves and three-dimensional clothoid interpolation is performed. There are seven unknowns a ₀ , a ₁ , a ₂ , b ₀ , b ₁ , b ₂ , h for each three-dimensional clothoid line segment if exactly passing through each point, so the unknowns are 7 (n-1 There will be). On the other hand, for the conditional expression, there are 7 coordinates, tangent, normal, curvature, and 3 coordinates at the end point for every n-2 connection points, so 7 (n-2) +3 in total It is a piece. In the method of 2-3, tangent vectors at the start and end points were given to this, and the number of conditional expressions and the number of unknowns were matched by increasing the number of conditions by four.

Here, to control the tangential-normal-curvature at the starting and end points, and if interpolated so that G ² continuous conditions than when a controlled tangential line at both ends, the normal-yet start and end points There will be a total of 4 additional curvatures, 2 each. The total number of conditional expressions is 7n-3. In this case, since the number of unknowns is smaller than the condition, a solution cannot be obtained by the Newton-Raphson method. Therefore, it is necessary to increase the unknowns by some method.

  Therefore, here, the number of unknowns and the number of conditional expressions are made equal by newly inserting interpolation target points. For example, if there are more four unknowns, two new points are inserted, and two of the coordinates of each point are treated as unknowns.

In this case, since the number of connection points is increased by 2, the condition for each connection point is increased by 14 including 7 coordinates, tangent, normal, and curvature. On the other hand, since the unknown number increases by two three-dimensional clothoid line segments, a total of 14 unknowns of 7 each of a ₀ , a ₁ , a ₂ , b ₀ , b ₁ , b ₂ , h increase. At this time, since the number of points included in the point sequence is n + 2, the total number of unknowns is 7 (n + 1) and the conditional expression is 7 (n + 1) +4. Furthermore, if two of the coordinates of the newly inserted point are treated as unknowns, the unknowns will increase by four. Then, there are 7 (n + 2) -3 unknowns and conditional expressions, and it becomes possible to find solutions for unknowns. With such an insert the new point, exactly as G ² continuously and the points given, it is possible to perform interpolation with a controlled tangential-normal-curvature end points.

Consider the more general case. When n points are interpolated, the number of points to be inserted and the number of coordinates handled as unknowns at that point when m items are controlled at both end points are considered. As mentioned earlier, if the curve is open, the point sequence is interpolated with n-1 curves. If exactly through each point, the unknowns for each 3D clothoid line segment are a ₀ , a ₁ , a ₂ , b ₀ ,
Since there are 7 b ₁ , b ₂ , and h, there are 7 (n-1) unknowns in total. On the other hand, for the conditional expression, there are 7 coordinates, tangent, normal, curvature, and 3 coordinates at the end point for every n-2 connection points, so 7 (n-2) +3 in total There are four fewer conditional expressions. In other words, there are four or more items to be controlled at both end points. In the description below, a method is described in which m is a natural number of 4 or more and k is a natural number of 2 or more, so that the conditional expression and the number of unknowns are equal when a new point is inserted.

(i) When m = 2k When controlling m = 2k items at both ends, the total number of unknowns is 7 (n-1), and the total number of conditional expressions is 7 (n-1) -4 + 2k It is a piece. At this time, there are 2k-4 excessive conditional expressions. Considering that k-2 points are newly inserted, there are k-2 3D clothoid line segments and k-2 connection points. ) And conditional expressions are 7 (n + k−3) −4 + 2k in total. Here, if two of the coordinate values of each newly inserted point are treated as unknowns (for example, x, y), the total number of unknowns is 7 (n + k-3) +2 (k-2). The total number of conditional expressions is 7 (n + k-3) +2 (k-2), and the number of unknowns and conditional expressions is equal.

(ii) When m = 2k + 1 When controlling m = 2k + 1 items at both ends, the total number of unknowns is 7 (n-1), and the total number of conditional expressions is 7 (n-1) + 2k-3 pieces. At this time, there are 2k-3 excessive conditional expressions. Now, considering that k-1 points are newly inserted, there are k-1 3D clothoid line segments and k-1 connection points.
The total number of unknowns is 7 (n + k-2), and the total number of conditional expressions is 7 (n + k-2) -3 + 2k. Here, if two of the coordinate values of each newly inserted point (for example, x, y) are treated as unknowns, the total number of unknowns is 7 (n + k-2) +2 (k-2). The total number of conditional expressions is 7 (n + k-2) + 2k-3, which increases the number of conditional expressions by one. Therefore, in the case of m = 2k + 1, it is assumed that only one of the coordinate values is treated as an unknown at one of the inserted points. By doing so, there are 7 (n + k-2) +2 (k-2) unknowns in total, and 7 (n + k-2) +2 (k-2) conditional expressions in total. The number of conditional expressions is equal.

  As in the method described above, the tangent rotation angle α other than tangent, normal, and curvature is controlled by adjusting the number of coordinates of the inserted point to be unknown according to the number of added conditions. Even in various cases, such as when to do, the number of unknowns and conditional expressions can be combined, and theoretically each value at both end points can be controlled. Table 5 summarizes the control items, the unknowns, and the number of conditional expressions.

(b) Method An interpolation method using a three-dimensional clothoid that controls each value at the start point and the end point is performed according to the following flow as shown in FIGS.

Step1) strictly through the interpolation target point by using only four of the control conditions, and to generate a curve subjected to G ² continuous interpolation.

  Step2) Insert a new point on the generated curve and adjust the conditional expression and the number of unknowns.

Using the curve parameters in Step 3) as initial values, approximate values of the parameters of each curve that satisfy the target condition are obtained by the Newton-Raphson method.

Hereinafter, a supplementary explanation will be given for each step. First, in Step 1, if the tangential direction is controlled, a curve is generated using the method (3). Even when the tangential direction is not controlled, the same initial value as that in the method (3) is used as the initial value when determining the parameter of the curve.

Next, in Step 2, a new point is inserted, and the condition and the number of unknowns are adjusted. At this time, the number of newly inserted points should be one or less as much as possible between each interpolation target point. In addition, as a point to be inserted, an intermediate point of the three-dimensional clothoid line segment generated in Step 1 connecting the interpolation target points was inserted. Furthermore, the points to be inserted are inserted in order from both ends. That is, the first insertion is between the start point and the adjacent point and between the end point and the adjacent point.

Finally, as for Step 3, it is necessary to newly determine an initial value for the Newton-Raphson method performed in Step 3. Therefore, the curve in which a new point is inserted is determined by dividing the curve using the method for dividing the three-dimensional clothoid curve described in 1-4, and determining the value from each value of the generated curve. For a curve with no points inserted, the value of the curve generated in Step 1 is used as it is. The initial value of each parameter of the curve in Step 3 was determined as described above. Using these initial values, a three-dimensional clothoid curve was generated from parameters obtained by the Newton-Raphson method, and interpolation was performed with a three-dimensional clothoid curve that satisfies the target conditions between point sequences.

(C) Interpolation Example An example of three-dimensional clothoid interpolation so that the tangent, normal, and curvature at both ends are controlled under the conditions shown in Table 6 is shown. Serial numbers were assigned to the points to be interpolated strictly, and P ₁ , P ₂ and P ₃ were assigned.

FIG. 16 shows the result of actual interpolation under these conditions. The solid line is a 3D clothoid curve,
A broken line, a one-dot chain line, a two-dot chain line, and a three-dot chain line indicate a curvature radius change pattern of each curve. FIG. 17 is a graph showing the relationship between the moving distance from the start point of each curve and the curvature corresponding to the types of curves in FIG. As can be seen from Table 7, the generated curve satisfies the given conditions.

(d) Control of values at midpoints
The method (b), while controlling the values of both end points, was able to perform the G ² consecutive interpolation. Here, consider a case where the value is controlled at the intermediate point instead of the end points.

For example, in the case of interpolating a sequence of points as shown in FIG. 18, given that controlling tangent, the normal at the intermediate point P _c. However, the method described so far cannot control the value at the midpoint. Therefore, here, the value at the intermediate point is controlled by dividing this point sequence into two.

That is, the interpolation is not performed on the point sequence at once, but is performed by dividing into the curves C ₁ and C ₂ across the intermediate point P _c . In this case, since the point _Pc corresponds to an end point, the value can be controlled by using the method (b).

  In this way, if you divide a section at a point with a value you want to control and connect the curves generated as a result of controlling and interpolating the values at both ends, you can theoretically control the tangent, normal, and curvature at each point. 3D clothoid interpolation can be performed.

(5) 3D clothoid interpolation with controlled tangent, normal and curvature at both ends
(a) Flow of method The interpolation method using a three-dimensional clothoid that controls each value at the start point and the end point is performed according to the following flow shown in FIG. In the following, description will be made along this flow.
(B-1) Give points to be interpolated In this example, three points in the three-dimensional space {0.0, 0.0, 0.0}, {5.0, 5.0, 10.0}, {10.0, 10.0, 5.0} are given. Other conditions such as tangent, normal, curvature, etc. given to each point are summarized in Table 8.

(b-2) Generation of 3DDCS with r = 4 In the Newton-Raphson method, it is necessary to give an appropriate initial value when starting the search for a solution. Here, preparation is made to obtain the initial value. The previous research, 3D Discrete Clothoid Splines, has the property that the curvature changes smoothly with respect to the moving distance from the starting point, strictly passing through the interpolation target point. Therefore, in this study, the initial value for 3D clothoid interpolation was determined by making a polygon Q of 3D Discrete Clothoid Splines with r = 4 as shown in Fig. 20, and calculating it from there. Further, polygons actually generated from this point sequence are shown in FIG. 21 and vertex coordinates are shown in Table 9.

(b-3) Determination of initial values In order to obtain a solution using the Newton-Raphson method, it is necessary to determine the initial values of each unknown. In this method, the value is determined by calculating the approximate value of each unknown using the polygon Q generated in b-2. In 3D Discrete Clothoid Splines, the Frene frame of each vertex has already been obtained. Therefore, parameters a ₀ and b ₀ are obtained from the unit tangent direction vector t of the polygon Q generated in b-2. This tangential direction vector t is already known when the polygon Q is obtained, and the tangential direction rotation angles α and β of the apex of the polygon Q are obtained from this t and the tangent equation of the three-dimensional clothoid curve. Thereby, initial values of a ₀ and b ₀ of each curve are obtained. For 3D clothoid segments starting from the start point, the value is given.

Here, 3D Discrete Clothoid Splines can be approximated to have a curve length variable S of 1/4 at a point q _{4i + 1} in FIG. 20, considering that the vertices are arranged at equal distances. Similarly, at the point q _{4 (i + 1) −1} , it can be approximated that the curve length variable S is 3/4. Considering these together with the formula of α of the three-dimensional clothoid curve, the following formula is established.

This equation is a two-dimensional simultaneous equation with unknown _numbers a1 _4i and a2 _4i , which are solved and set as initial values of parameters a ₁ and a ₂ . Similarly, the initial values of the parameters b ₁ and b ₂ can be determined.

  The remaining unknown is the curve length h. The initial value is calculated from the curvature equation of the three-dimensional clothoid curve. The curvature of the 3D clothoid curve is expressed as follows.

  By transforming this equation, the following equation is obtained, and the initial value of h is determined.

  With the above method, initial values can be determined for seven three-dimensional clothoid parameters.

  Table 10 shows the initial values actually obtained by this method.

(b-4) G ² continuous 3D clothoid interpolation strictly passing through each point
(b-3) using the initial value determined by obtaining the Newton-Raphson method to an approximation of the parameter of each curve under conditions such that G ² continuous. From the parameters obtained by this, 3
A three-dimensional clothoid segment was generated and interpolated between point sequences with a three-dimensional clothoid curve.

Here, in the three-point three-dimensional clothoid interpolation, a specific condition is considered for a condition that exactly passes through the interpolation target point and is G ² continuous. FIG. 22 shows three-dimensional clothoid interpolation of points P ₁ , P ₂ , P ₃ . Assuming that the curve connecting the points P ₁ and P ₂ is the curve C ₁ and the curve connecting the points P ₂ and P ₃ is the curve C ₂ , since a0 ₁ and b0 ₁ are already known, the unknown is the curve C ₁ 12 parameters a1 ₁ , a2 ₁ , b1 ₁ , b2 ₁ , h ₁ , and parameters a0 ₂ , a1 ₂ , a2 ₂ , b0 ₂ , b1 ₂ , b2 ₂ , h ₂ of the curve C ₂ . The character subscripts described below correspond to the subscripts of each curve. The coordinates, tangent rotation angles α, β, normals, and curvatures of each curve are used as functions of the curve length variable S as Px _i , Py _i , Pz _i , α _i , β _i , n _i , and κ _i are expressed as follows.

First, the condition that the point P ₁ strictly passes through the point to be interpolated is inevitably achieved when the starting point is given in view of the definition of the three-dimensional clothoid curve. Since the tangential direction is also given as a known value, the condition at the point P ₁ is not specified.

Then think about the point P _2. Point P ₂ is a connection point between curves, and the position, tangent, normal, and curvature must be continuous in order to be G ² continuous. That is, the condition that should be satisfied at the point P ₂ is as follows.

Finally, think about the point P _3. Point P ₃ is the end point, the condition to be satisfied is located, because it is tangent only the following five conditions is satisfied. Here, α ₃ and β ₃ are tangential direction rotation angles α and β that determine a tangent vector at the given end point.

From the above, unknowns a1 ₁ , a2 ₁ , b1 ₁ , b2 ₁ , h ₁ , a0 ₂ , a1 ₂ , a2 ₂ , b0 ₂ , b1 ₂ , b2 ₂ , h ₂ 12
It can be seen that the following 12 conditional expressions hold for each. In summary, the conditional expressions that hold are as follows.

  Since 12 equations are established for 12 unknowns, a solution can be obtained. This equation was solved by the Newton-Raphson method to find a solution. Table 11 shows initial values and solutions.

(b-5) Generation of Curve FIG. 23 shows the curve generated based on the parameter obtained in (b-4) and the polygon generated in b-2 at the same time. The solid curve is the curve C ₁ , and the dashed curve is the curve C ₂ . At this stage, it is a G2 continuous 3D clothoid curve with the tangent direction controlled at the start and end points.

(b-6) Conditional expression and unknown number Here, it is considered that the normal and curvature at the start point P ₁ and the end point P ₃ are also set to the values given in Table 8. To further control the normal and curvature at the start and end points, it is necessary to increase the conditions at the start and end points by two. However, when the number of conditions increases by four, a solution that satisfies the condition cannot be obtained from the relationship with the number of unknowns. Therefore, in order to match the unknown and the number of conditional expressions, a point DP ₁ is newly inserted at the position of the curve length variable S = 0.5 of the curve C ₁ as shown in FIG. For the curve C ₂ , a point DP ₂ is newly inserted at the position of the curve length variable S = 0.5.

At this time, the curve connecting point P ₁ and point DP ₁ is curve C ′ ₁ , the curve connecting point DP ₁ and point P ₂ is curve C ′ ₂ , and the curve connecting point P ₂ and point DP ₂ is curve C ′ _3. A curve connecting the points DP ₂ and P ₃ is defined as a curve C ′ ₄ . The character subscripts that appear in the description below correspond to each curve name, for example, the coordinates in curve C,
Px _c , Py _c , Pz _c , α _c , β _c , n _c , with tangent rotation angles α, β, normal, and curvature as a function of curve length variable S
expressed as κ _c. At the start and end points, the coordinates, tangential rotation angles α, β, normal, and curvature are set to Px _s , Py _s , Pz _s , α _s , β _s , n _s , κ _{s at} the start point, and Px _e , Py _e , Pz _e , α _e , β _e , n _e ,
It is expressed as κ _e .

  The conditions that hold at each point are described below.

From the above, there are 32 conditional expressions that should be satisfied as a whole. Here, the clothoid parameters of each curve are 7 each of a ₀ , a ₁ , a ₂ , b ₀ , b ₁ , b ₂ , h, and the number of unknowns is 28 because there are 4 curves. However, since the number of unknowns is not equal to the number of conditional expressions, a solution cannot be obtained. Therefore, the y and z coordinates of the two newly inserted points DP ₁ and DP ₂ were treated as unknowns, and the unknowns were increased by four. By doing so, there are 32 unknowns and conditional expressions, and a solution can be obtained.

(b-7) Determination of initial value 2
The Newton-Raphson method is used to find a solution that satisfies the conditional expression established in (b-6).
In order to increase the convergence rate, an initial value of unknown is determined. As a method, as shown in FIG. 25, by dividing the 3D clothoid curve generated in (b-5) before and after the newly inserted point,
Four dimensional clothoid curves were made and their clothoid parameters were given.

Regarding the method of dividing the curve, the method of dividing the curve C ₁ into the curve C ′ ₁ and the curve C ′ ₂ will be described. The clothoid parameters h ′, a ′ ₀ , a ′ ₁ , a ′ _{2 of the} curve C ′ ₁ , b ′ ₀ , b ′ ₁ , b ′ ₂ can be expressed by the following equations using the parameters of the curve C ₁ . Here, S _d is a curve length variable at the dividing point, and is 0.5 here.

Next, consider a curve C ′ ₂ starting from the dividing point DP ₁ . First, if a curve C '' ₁ is the same size and shape as the curve C ₁ but the opposite direction, the curve is the clothoid parameter h '', a '' ₀ , a '' ₁ , a '' ₂ , b ″ ₀ , b ″ ₁ , b ″ ₂ can be expressed by the following equation using the parameters of the curve C ₁ .

On this curve, the dividing point DP ₁ is represented by DP ₁ = C ″ ₁ (1-S _d ). Here, considering that the curve C '' ₁ is divided at the point DP ₁ , the curve C '' starting from the point P ₂ among the divided curves is shown.
₂ is a curve having the same size and shape as the curve C ′ ₂ but opposite in direction. _'2' curve C by the method that generated ₁ 'curve C can be produced. Here, if a curve having the same size and shape with respect to the curve C ″ ₂ but having the opposite direction is generated, the curve C ₂ can be generated.

The clothoid parameter h '', a '' ₀ , a '' ₁ , a '' ₂ , b '' ₀ , b '' ₁ , of this curve C ₂
b ″ ₂ is expressed by the following equation using the parameters of the curve C ₀ .

In the above method, the curve C ₁ in a three-dimensional clothoid curve C ₁ on the curve length variable S = 0.5 at which point DP ₁
Can be divided into curves C ′ ₁ and C ′ ₂ . In a similar manner, it is possible to curve length variables on the curve C ₂ divides the curve C ₂ in DP ₂ points are S = 0.5 to the curve C _'3 and C' _4.

The parameters of the four curves divided by this method are listed in Table 12. The parameters of this curve were used as the initial values for the Newton-Raphson method when finding a solution that satisfies the conditional expression established in b-6.

(B-8) Find a clothoid parameter that satisfies the conditions
Based on the initial value determined in (b-7), a solution satisfying the conditional expression established in (b-6) was obtained by the Newton-Raphson method. Table 13 shows the calculated parameters of each curve. Table 14 shows the difference between the given value and the tangent, normal, and curvature of the start and end points of the generated curve.

(b-9) Curve generation
FIG. 26 shows a curve generated with the parameters obtained in (b-8). The solid line is a three-dimensional clothoid curve. The broken line, alternate long and short dashed lines, two-dot chain line, and three-dot chain line are the curvature radius change pattern in which the direction of each curve is the main normal direction and the magnitude is the radius of curvature plus the natural logarithm. Is shown. FIG. 27 is a graph showing the relationship between the moving distance s from the starting point of each curve and the curvature κ corresponding to the type of line in FIG. As can be seen from Table 12, the generated curve satisfies the given conditions.

The above describes an example of curve generation by the three-dimensional clothoid interpolation method in which the tangent, normal, and curvature are controlled at both ends.

3. Numerical control method using three-dimensional clothoid interpolation The above-mentioned three-dimensional clothoid interpolation curve is effectively used for generating numerical control information for controlling the motion of a tool of a machine tool and other moving objects. The feature is that speed control is easy and speed change can be smoothed.

(1) Numerical control method using three-dimensional clothoid interpolation The numerical control method using a three-dimensional clothoid interpolation curve consists of the following procedures shown in FIG.

(a) Tool motion trajectory design (Fig. 28, S1)
The method described in the previous section is used to determine the 3D clothoid interpolation curve that satisfies the conditions. When a tool such as a robot moves, the representative point of the tool (tool center point) is considered to move in time on a continuous trajectory curve (including straight lines) drawn planarly or spatially. Can do. The position of the tool point is represented by coordinates (x, y, z), and the posture of the tool point is represented by a rotation angle with respect to the x, y, z axis, for example. In any complicated movement, the tool point trajectory is continuously connected without interruption. The first stage of motion control is to design the shape of this locus into a three-dimensional clothoid curve.

(b) Fit the motion curve (Fig. 28, S2)
According to the request from numerical control, the distribution of the moving speed of the control target point on the curve is specified along the 3D clothoid interpolation curve. That is, the second stage of motion control is to determine the speed / acceleration of the tool point moving on the designed locus. How the tool point moves as a function of time on the trajectory is determined by determining the speed and acceleration of the tool point. The speed / acceleration of the tool point may be determined with respect to time or may be determined accompanying the shape of the trajectory. In general, it is often determined with respect to time, but for example, when processing a curved surface, the shape of the trajectory is changed from a request to move at a high speed in a flat part and at a low speed in a bent part. Concomitantly, the speed is determined.

  In the present embodiment, for example, a curve having good characteristics adopted in the cam mechanism is adopted. Positions and postures defined in the Cartesian space (real space) constitute a continuous curve group, and an acceleration / deceleration is designated by applying a motion curve to each curve. The Cartesian space is a three-dimensional coordinate system created using three axes x, y, and z that are orthogonal to each other at the origin, and can represent not only the position of the tool point but also the posture.

(c) Time division (FIG. 28, S3) and calculation of tool position / orientation using Cartesian coordinate system (
(FIG. 8, S4)
Here, for each unit time for calculating the numerical control information, the movement position and orientation of the tool point are calculated in accordance with the designated movement speed of the control target. Since the trajectory and motion are determined, the position / orientation of the tool point is given as a function of time t. Thereby, when the time t is given at a minute time interval, the displacement of the tool point with respect to each time can be obtained. Specifically, the calculation in (c) is performed as follows. At the current point, values such as position information, tangent, and curvature are known. If the specified moving speed is multiplied by the unit time, the moving curve length during the unit time can be obtained, and the curve length parameter after movement can be calculated. Based on the curve length parameter after movement, values such as position information, tangent, and curvature at the point after movement can be calculated.

With the above procedure, the position and orientation of the tool point with respect to time t in the Cartesian coordinate system (real space) are calculated. The variables are (x, y, z, λ, μ, ν, θ) in three dimensions. However, (λ, μ, ν, θ) represents the posture E in equivalent rotation (λ, μ, ν
) Represents an equivalent rotation axis, and θ represents a rotation angle.

  Further, an offset point offset by a specified dimension in the normal direction is obtained along a three-dimensional clothoid interpolation curve according to a request from numerical control, and this is used as a cutter path (tool center trajectory). This calculation is also easy because the normal direction is determined.

(d) Reverse mechanism solution (Fig. 28, S5)
Next, the rotation angle of each axis necessary to give the position / posture of the tool point is obtained. This process is generally called inverse kinematics. For example, if there is a six-axis robot, there are six joints, so the position / posture of the tool point is determined by how many times the shoulder joint, arm joint, elbow joint, wrist joint, etc. have been rotated. This is called the forward mechanism solution. On the contrary, the reverse mechanism solution is to obtain the rotation angles θ1 to θ6 of the axial space from the position / posture of the actual space. The actuator for each axis is not necessarily a rotary motor, but may be a linear actuator such as a linear motor, but even in that case, the calculation of electronic gears that convert the actual displacement to the number of input pulses of the linear motor at the minimum is still possible. Necessary. Since the reverse mechanism solution is unique to each type of mechanism such as a robot, a solution is prepared for each of various robots.

(e) Calculation of motor displacement of each axis in the axis coordinate system (FIG. 28, S6)
An inverse mechanism solution is obtained for each time-divided tool point, and this is converted into an integer as a displacement pulse of each axis motor (including a linear actuator). When the pulse control is not used, the data is obtained as integerized data corresponding to the number of pulses using the minimum resolution unit (resolution) of each axial displacement.

  The above (a) and (b) are preliminary procedures and are performed only once. Steps (c) to (e) are executed every specified unit time, and are continued until the target time or the target condition is satisfied.

  All of the above calculations can be performed in a numerical controller, or (a) and (b) are calculated and set by another computer (computer), and the curve parameters are numerically set. The calculation of (c) to (e) can also be performed in the numerical control device.

(2) NC device and CNC device Hereinafter, a case where an independent numerical control device (NC device) is used and a case where a CNC device in which a computer having the role of a programmer and the NC device are integrated will be described. .
(A) In the case of using an independent NC device In a conventional ordinary NC machine, hardware is composed of two devices: a programmer that creates NC data by programming and an NC device that moves the machine using this NC data. Are separated. On the other hand, in a recent CNC machine, a computer that performs programming is integrated in the NC apparatus.

  First, a numerical control method using a three-dimensional clothoid is proposed for the former case where an independent NC device is used. In this case, clothoid data is exchanged using clothoid parameters, and a clothoid format is defined in the G code. This is, for example, as follows.

G ***
A0, A1, A2, B0, B1, B2, H
Here, G *** indicates a G code number. A0 ~ H is 7 of 3D clothoid segment
Indicates one parameter. Before you run this code, the tool has come to the position of P _0. The NC device uses this parameter to calculate and execute an instantaneous tool position or a tool position difference. This operation is called “order solution”. The reason for performing the forward solution on the NC device side is to prevent the data from becoming too large. For this purpose, the NC device requires some kind of calculation. By expressing clothoid with G code, it is possible to incorporate clothoid curves into existing NC devices.

(b) CNC method The case of a CNC device in which a computer having the role of a programmer and an NC device are integrated will be described. In this case, it does not matter which part of the hardware is used to calculate the clothoid. In addition, the amount of data and the speed of transfer are being solved.

  In general, the programmer includes a process of determining the proper clothoid parameters for each condition. This is called “reverse solution”. The inverse solution includes, for example, a program (free point sequence interpolation) that gives some discrete point sequences and calculates a smooth curve that exactly passes through these points. In addition, a tool path determination program (so-called CAM) necessary for machining is often included.

(3) Features of numerical control method using three-dimensional clothoid interpolation The numerical control method using three-dimensional clothoid interpolation has the following advantages.

(a) As described above, since the curve is expressed with the curve length from the reference point as an independent parameter, numerical control information corresponding to the designated moving speed can be generated. In other curves such as a spline curve expressed by an independent parameter unrelated to the curve length, even if the point after movement can be calculated, it is difficult to calculate the value of the independent parameter corresponding to that point, It is not easy to generate numerical control information corresponding to the designated moving speed.

In detail, as shown in FIG. 29, consider a case where the tool is moved at a certain linear velocity from a point _R0 on the locus represented by the spline curve R (t). When the target point of the tool is calculated at regular time intervals, the tool movement amount ΔS after the unit time elapses can be known, but the independent variable t is not related to the time or the curve length. Δt cannot be obtained immediately. Unless Δt is obtained by solving the equation R ₀ + ΔS = R (t ₀ + Δt), the target point cannot be calculated. Therefore, this calculation must be repeated at regular time intervals.

  (b) In the 3D clothoid curve, it is expected that the curvature change with respect to the curve length is approximately constant, and the numerical control information corresponding to this is difficult from the viewpoint of motion control. Expected to be less control information. In general spline interpolation or the like, it is difficult to predict and control the change in curvature.

(c) 3D clothoid curves include straight lines, arcs, spiral curves, etc. as special cases, and it is possible to accurately express numerical control information for various curves without incorporating individual curve formulas. I can do it.

(d) The 3D clothoid curve is a natural equation that does not depend on the coordinate axes. In conventional NC devices where the curves are represented by the x, y, and z axes, for example, when machining a workpiece obliquely, depending on how the workpiece is attached, it may be difficult or difficult to machine an oblique surface. There are things to do. In a three-dimensional clothoid curve, the curve is given by the line length. Therefore, even when processing an oblique surface, if a locus is created on the oblique surface, it can be processed in the same manner as processing a horizontal plane.

Note that the tool trajectory or the contour shape of the workpiece can be obtained by using a three-dimensional curve (referred to as a three-dimensional clothoid curve) in which the pitch angle and the yaw angle in the tangential direction are given by a quadratic expression of a curve length or a curve length variable according to the present invention. When a program to be expressed is executed by a computer, the program is stored in an auxiliary storage device such as a hard disk device of the computer, loaded into the main memory and executed. In addition, such a program is stored in a portable recording medium such as a CD-ROM for sale, or stored in a recording device of a computer connected via a network, and transmitted to another computer via the network. It can also be transferred. Moreover, the calculation results (curve parameters of a three-dimensional clothoid curve, displacement pulses of each axis motor, etc.) by such a program can be stored and sold in a portable recording medium such as a CD-ROM.

  According to the three-dimensional clothoid curve of the present invention, it is possible to provide a general method for generating a spatial curve necessary for the design and production of industrial products. When an object moves with acceleration / deceleration along a space curve, it is possible to design with a smooth change in restraining force. This feature is widely applied to numerical control devices. In addition to the numerical control device, the ability to appropriately design the change in curvature with respect to the curve length enables effective application to various industrial fields such as aesthetic design curve design.

The figure which shows the two-dimensional clothoid curve on x and y coordinate. The figure which shows a two-dimensional clothoid curve. The figure which shows the definition of (alpha) and (beta) of a three-dimensional clothoid curve. The figure which shows the shape of a typical three-dimensional clothoid curve. Shows the G ² consecutive interpolation conditions. The figure which shows the concept of a contact plane. The figure which shows the outline | summary of the flow of the method of clothoid interpolation. Shows a satisfying clothoid overview of the flow of interpolation techniques such as the G ² continuous. Shows a three-dimensional clothoid interpolation of points _{P 1, P 2, P 3} . The figure which shows 3D Discrete Clothoid Splines of r = 4. The figure explaining 3D Discrete Clothoid Splines. A perspective view of a 3D clothoid curve generated by interpolation. A curvature change graph in which the horizontal axis represents the movement distance from the start point and the vertical axis represents the curvature. The figure which shows the outline | summary of the flow of the three-dimensional clothoid interpolation which controls each value by a both-ends point. Schematic of 3D clothoid interpolation that controls each value at both end points. The figure which shows the result of actually performing interpolation. A graph of the relationship between the moving distance from the start point of each curve and the curvature. The figure which shows control of the value in an intermediate point. The figure which shows the outline | summary of the flow of the interpolation method using the three-dimensional clothoid which controls each value by a start point and an end point. The figure which shows 3D Discrete Clothoid Splines of r = 4. The figure which shows the produced | generated polygon. Shows a three-dimensional clothoid interpolation of points _{P 1, P 2, P 3} . The figure which shows the produced | generated curve and polygon. The figure which inserted the point. The figure which shows the divided | segmented three-dimensional clothoid curve. The figure which shows the produced | generated curve. The graph which shows the relationship between the movement distance s from the starting point of each curve, and curvature κ. Process drawing which shows a numerical control method. The comparison figure which shows the conventional spline curve.

Claims (7)

  A 3D clothoid curve (3D clothoid segment) in which the pitch angle and yaw angle in the tangential direction are each given by a quadratic expression of the curve length or the curve length variable, between points given arbitrarily in 3D coordinates. A numerical control method in which interpolation is performed and the movement of the tool is controlled by the three-dimensional clothoid segment.   Connect multiple three-dimensional clothoid curves (three-dimensional clothoid segments) in which the pitch angle and yaw angle in the tangential direction are given by the curve length or the quadratic equation of the curve length variable, and the tools are connected by these three-dimensional clothoid segments. Numerical control method to control the movement of the body. The numerical control method according to claim 1, wherein the three-dimensional clothoid curve is defined by the following formula.

here,

Indicates the position vector of the point on the three-dimensional clothoid curve and its initial value.
Let the length of the curve from the start point be s, and the total length (the length from the start point to the end point) be h. The value obtained by dividing s by h is represented by S. S is a dimensionless value and is called a curve length variable.
i, j, and k are unit vectors in the x-axis, y-axis, and z-axis directions, respectively.
u is a unit vector indicating the tangent direction of the curve at point P, and is given by equation (2). E ^kβ and E ^jα are rotation matrices representing the rotation of the angle β around the k axis and the rotation of the angle α around the j axis, respectively. The former is called yaw rotation, and the latter is called pitch rotation. Equation (2) shows that the tangent vector u is obtained by first rotating the unit vector in the i-axis direction by α around the j-axis and then by β around the k-axis.
a ₀ , a ₁ , a ₂ , b ₀ , b ₁ , b ₂ are constants. The seven parameters a ₀ , a _{1 of the} three-dimensional clothoid segment are set so that the positions and tangential directions (and sometimes the curvature) of the three-dimensional clothoid segment are continuous at the joint of one three-dimensional clothoid segment and the next three-dimensional clothoid segment. , A ₂ , b ₀ , b ₁ , b ₂ , h are calculated, the numerical control method according to claim 3.   Interpolate between point sequences arbitrarily given in 3D coordinates using 3D clothoid segments in which the tangential pitch angle and yaw angle are given by the curve length or the quadratic expression of the curve length variable. A numerical controller that controls the movement of a tool by means of a three-dimensional clothoid segment. To numerically control the movement of the tool,
Computer
As means for interpolating between a sequence of points arbitrarily given in three-dimensional coordinates using a three-dimensional clothoid segment in which the pitch angle and yaw angle in the tangential direction are each given by a quadratic expression of a curve length or a curve length variable A program to make it work. To numerically control the movement of the tool,
Computer
As means for interpolating between a sequence of points arbitrarily given in three-dimensional coordinates using a three-dimensional clothoid segment in which the pitch angle and yaw angle in the tangential direction are each given by a quadratic expression of a curve length or a curve length variable A computer-readable recording medium on which a program for functioning or a calculation result by the program is recorded.

Images