WO2018205276A1 - Motion trajectory smooth transition method and device and related equipment - Google Patents

Abstract

Provided are a method and device for smooth transition of motion trajectory and related equipment. The method for smooth transition of the motion trajectory comprises: obtaining a first trajectory (NP) and a second trajectory (PM) to be smoothly transferred and a preset contour error (E) (S11); determining a position of a feature interpolation point (Q) according to the first trajectory (NP), the second trajectory (PM) and the preset contour error (E) (S12); and according to the first trajectory (NP), the second trajectory (PM), the feature interpolation point (Q) and a general equation of interpolation curve, fitting an interpolation curve connecting the first trajectory (NP), the feature interpolation point (Q) and the second trajectory (PM). The method and device for smooth transition of the motion trajectory can accurately fit the interpolation curve, which realizes continuous curvature and speed, and can ensure the contour error of the equipment.

Description

Method and device for smoothing of motion track and related equipment [Technical Field]

The present invention relates to the field of motion control technologies, and in particular, to a method and apparatus for smooth transition of motion trajectories and related devices.

【Background technique】

In many cases, there is usually an angle between the two adjacent motion trajectories. When moving to this point, the direction of the feed speed is abrupt. If the speed is kept constant, a large acceleration will be generated. Acceleration may exceed the acceleration capability of the device, causing the corresponding device to vibrate. In order to make this acceleration less than the maximum acceleration of the system, it is necessary to reduce the speed at the connection point of the two sections of motion trajectory, and a large number of motion trajectories are accumulated, so that the motion efficiency is seriously affected.

For this reason, it has been proposed to introduce an interpolation curve between the two trajectories where the angle exists, and to use the "rounded corners" to approximate the "sharp angle", thereby avoiding the need to reduce the speed at the sharp corners. Different interpolation curves have a basic uniform requirement, that is, the position of the connection point between the interpolation curve and the above two trajectories ensures that C ^{1 is} continuous (the first derivative is continuous), so that the speed is continuous. Further, we hope that the curvature at the joint is also continuous, that is, C ^{2 is} continuous (the second derivative is continuous), which can ensure that the acceleration does not cause jumps to cause equipment vibration, and ensure the smoothness of the processing while ensuring the processing efficiency. Can further improve system performance.

The existing interpolation curve is usually an approximate arc. Although the arc calculation method is simple, it cannot guarantee that C ^{2 is} continuous at the connection point between the arc and the two segments, and only C ^{1 is} continuous.

When the above two trajectories are interpolated by using other curves, since the calculation conditions are insufficient, the equation of the curve cannot be accurately determined, and at most, the interpolation curve can be approximated.

In addition, the equipment usually sets a contour error. At present, the contour error is usually not guaranteed when calculating the interpolation curve, which leads to the problem that the motion of the motion curve is not smooth due to the smoothness of the motion curve. .

Therefore, there is a need to provide a method and apparatus for smooth transition of motion trajectories and related equipment to solve the above technical problems.

[Summary of the Invention]

The technical problem to be solved by the present invention is to provide a method and device for smooth transition of motion trajectory and related equipment, which can accurately fit an interpolation curve that achieves continuous curvature and speed and can ensure contour error of the device.

In order to solve the above technical problem, a technical solution adopted by the present invention is to provide a method for smoothly switching a motion trajectory, the method comprising: acquiring a first trajectory and a second trajectory to be smoothly transferred and a preset contour error; The first trajectory and the second trajectory and the preset contour error determine the position of the feature interpolation point; the first trajectory and the feature interpolation are fitted according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation The interpolation curve of the point and the second trajectory, so that the curvature and tangential angle of the first trajectory and the interpolation curve at the joint point of the two trajectories are equal, and the curvature and tangential angle of the second trajectory and the interpolation curve at the joint point of the two points Equal; the first and second tracks are smoothly transferred using the interpolation curve.

In order to solve the above technical problem, a technical solution adopted by the present invention is: a device for smoothly switching a motion trajectory, the device comprising a processor and a memory connected to the processor, the memory for storing the first track, the second track, and the pre- Setting a contour error, the processor is configured to acquire a first trajectory, a second trajectory, and a preset contour error from the memory; determine a position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error; according to the first trajectory, Two trajectories, feature interpolation points, and general equations for interpolation curves And inserting an interpolation curve connecting the first trajectory, the feature interpolation point and the second trajectory, so that the curvature of the first trajectory and the interpolation curve at the connection point of the two is equal to the tangential angle, and the second trajectory and the interpolation curve are The curvature of the joint points is equal to the tangential angle.

In order to solve the above technical problem, another technical solution adopted by the present invention is to provide a storage device that stores program data that can be executed to implement the above method.

In order to solve the above technical problem, another technical solution adopted by the present invention is to provide a robot system including a robot and a controller connected to the robot, and the controller is configured to acquire a first track and a second to be smoothly transferred. a trajectory and a preset contour error; determining a position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error; and fitting according to the first trajectory, the second trajectory, the feature interpolation point, and the general equation of the interpolation curve Interpolating the interpolation curve of the first trajectory, the feature interpolation point and the second trajectory such that the curvature of the first trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle, and the second trajectory and the interpolation curve are in both The curvature of the connection point is equal to the tangential angle; and the motion of the robot or the components of the robot is controlled according to the first trajectory, the second trajectory, and the interpolation curve.

In order to solve the above technical problem, another technical solution adopted by the present invention is to provide a numerical control machine tool including a machine tool body and a numerical control device, and the numerical control device is configured to acquire a first track and a second track to be smoothly transferred and Presetting the contour error; determining the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error; fitting the connection according to the first trajectory, the second trajectory, the feature interpolation point, and the general equation of the interpolation curve Interpolation curve of a trajectory, a feature interpolation point and a second trajectory, so that the curvature and tangential angle of the first trajectory and the interpolation curve at the connection point of the two are equal, and the second trajectory and the interpolation curve are at the connection point of the two The curvature is equal to the tangential angle; the motion of the machine body or the components of the machine body is controlled according to the first trajectory, the second trajectory, and the interpolation curve.

The beneficial effects of the present invention are: different from the prior art, the present invention determines the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error by using the contour error as a condition for fitting the interpolation curve. Then, according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation, the interpolation curve connecting the first trajectory, the feature interpolation point and the second trajectory is fitted, and on the one hand, the accurate fitting can be accurately performed. The interpolation curve, on the other hand, the fitted interpolation curve ensures the contour error of the device during motion.

[Description of the Drawings]

1 is a flow chart of a method for smoothing a motion track according to a first embodiment of the present invention;

2 is a flow chart of a method for smoothing a motion track according to a second embodiment of the present invention;

Figure 3 is a geometrical diagram of line segments, arcs, and interpolation curves;

4 is a flow chart of a method for smoothing a motion track according to a third embodiment of the present invention;

Figure 5 is a geometrical diagram of the first arc, the second arc, and the interpolation curve;

6 is a schematic block diagram of a motion trajectory smoothing switching device according to an embodiment of the present invention;

7 is a schematic diagram of a storage device according to an embodiment of the present invention;

8 is a block diagram of a robot system according to an embodiment of the present invention;

Figure 9 is a block diagram of a numerically controlled machine tool according to an embodiment of the present invention.

【detailed description】

The invention will now be described in detail in conjunction with the drawings and embodiments.

Please refer to FIG. 1. FIG. 1 is a flow chart of a method for smoothing a motion track according to a first embodiment of the present invention. In this embodiment, the method for smoothing the motion track includes the following steps:

Step S11: Acquire a first track and a second track to be smoothly transferred and a preset contour error.

In this embodiment, the first track is an arc or a line segment, and the second track is an arc. For details, refer to the description below.

Step S12: determining the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error.

In this embodiment, the determining the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error may be: a tangential angle at a connection point of the first trajectory and the second trajectory On the angle bisector, the point at which the connection point between the first trajectory and the second trajectory is the preset contour error is selected as the feature interpolation point. For details, refer to the description below.

Step S13: fitting an interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation, so that the first trajectory and the insertion The curvature of the complementary curve is equal to the tangential angle at the joint point of the two, and the curvature and tangential angle of the second trajectory and the interpolation curve at the joint point of the two are equal. For details, please refer to the description below.

In this embodiment, the step of fitting the interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation includes: : fitting a first sub-curve connecting the first trajectory and the feature interpolation point and a second sub-curve connecting the feature interpolation point and the second trajectory, wherein the first sub-curve and the second sub-curve respectively satisfy the interpolation curve The general equations have different curve parameters. The first sub-curve and the second sub-curve have the same curvature and tangential angle at the feature interpolation point. For details, please refer to the description below.

In the present embodiment, the fitting is performed in a manner of dividing into two sub-curves. In other embodiments, the method may be performed by dividing into three or more sub-curves, or by using a curve.

In this embodiment, the first sub-curve is a first clothoid curve, and the second sub-curve is a second convoluted curve. In other embodiments, the first sub-curve and the second sub-curve may also adopt other characteristic curves.

Referring to FIG. 2, FIG. 2 is a flowchart of a method for smoothing a motion track according to a second embodiment of the present invention.

In this embodiment, preferably, the step of connecting the interpolation curves connecting the first trajectory, the feature interpolation point, and the second trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation The method includes: fitting a first sub-curve connecting the first trajectory and the feature interpolation point, and a second sub-curve connecting the feature interpolation point and the second trajectory, wherein the first sub-curve and the second sub-curve respectively satisfy the interpolation The general equation of the curve has different curve parameters, and the curvatures and tangential angles of the first sub-curve and the second sub-curve at the feature interpolation point are equal. The curvature of the first trajectory and the interpolation curve at the joint point of the two trajectories is equal to the tangential angle, and the curvature of the second trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle. The first track is a line segment and the second track is an arc. The first sub-curve is a first clothoid curve and the second sub-curve is a second convoluted curve.

In this embodiment, the method for smoothing the motion track includes the following steps:

Step S21: Obtain a line segment and an arc to be smoothly transferred and a preset contour error.

In this embodiment, the acquiring the line segment and the arc to be smoothly transferred includes: acquiring the position of the line segment and the position of the arc and the positional relationship between the two.

Step S22: determining the position of the feature interpolation point according to the line segment and the arc and the preset contour error.

In this embodiment, the determining condition of the feature interpolation point is: selecting a distance line segment and an arc connecting point of the angle between the intersection line of the arc and the line segment and the angle of the line segment as the preset contour error. The point is used as a feature interpolation point. That is, the Q point in FIG. 3 or FIG. 4, see the description below.

Step S23: setting the line segment on a coordinate axis of the Cartesian coordinate system, and the end of the line segment not connected to the arc is set to coincide with the origin of the Cartesian coordinate system;

That is, one end of the line segment that is not connected to the arc is taken as the origin, and a coordinate system is established with the line segment as one coordinate axis, as described in detail below.

Step S24: Solving the curve parameters of the first convoluted curve and the second convoluted curve by combining the following two sets of equations:

Where x ₀ is the coordinate of the connection point of the first convoluted curve and the line segment on the coordinate axis, c ₀ is the curvature change rate of the first convoluted curve, and s ₀ is the connection point of the first convoluted curve from the line segment to the feature insertion The arc length between the complement points, c ₁ is the curvature change rate of the second convolution curve, and s ₁ is the arc length of the second convolution curve from the feature interpolation point to the connection point with the arc, and α is an arc from the connection point with the second clothoid arc to the central angle of the arc segment and the connection point between the arc segment, T _ccw rotation _{matrix, T ccw, x 0, c} 0, s 0, c 1, s ₁ and α are unknown quantities that need to be solved by two sets of equations;

Where L is the length of the line segment, E is the preset contour error, β is the angle between the line segment and the arc, R is the radius of the arc, x _c is the coordinate of the center of the arc on the coordinate axis, and y _c is the circle The coordinates of the center of the arc on another coordinate axis of the Cartesian coordinate system,

The direction vector of the point from the center of the arc to the arc and the line segment, x _c , y _c ,

L, β, and R are known quantities that can be obtained by the relative positional relationship between the line segment and the arc and both.

The derivation process is explained below.

Firstly, the general equation of the interpolation curve is introduced. The interpolation curve is a clothoid curve, which has the characteristics that the curvature varies linearly with the arc length of the path.

There are three general expressions for the interpolation curve:

k(s)=k ₀ +cs (3)

Where s is the curve path arc length, x(s) is the curve path arc length corresponding to the abscissa, x ₀ is the abscissa at the beginning of the curve, y ₀ is the abscissa at the beginning of the curve, y(s) The arc length of the curve path is the ordinate corresponding to s, θ(s) is the tangential angle corresponding to the arc length of the curve path, θ ₀ is the tangential angle of the starting point of the curve, and k ₀ is the curvature of the starting point of the curve, c For the rate of curvature change of the curve, k(s) is the curvature of the curved path with the arc length being s.

Next, the determination of the position of the feature interpolation point and the derivation of the equations (1) and equations (2) are further explained. It is noted that the curvature and the tangential angle of the first trajectory and the interpolation curve are equal to the first smooth transition condition, and the curvature and tangential angle of the second trajectory and the interpolation curve at the joint point are equal to the second Smoothing the transition condition, the curvatures and tangential angles of the first sub-curve and the second sub-curve at the feature interpolation point are equal to a third smooth transition condition.

Please refer to FIG. 3. FIG. 3 is a geometric diagram of line segments, arcs, and interpolation curves.

The smooth transition of the line segment NP and the arc PM will be described below as an example. The I point is the connection point between the line segment NP and the first convolution curve IQ, the O point is the connection point of the arc PM and the second convolution curve QO, the P point is the connection point of the line segment NP and the arc PM, and C is the arc PM. Center of the circle. For the direction of motion, take N point → I point → Q point → O point → M point as an example.

Let the central angle corresponding to the arc segment OP between the P point and the O point be α.

In step S21, acquiring line segments and arcs to be smoothly transferred and preset contour errors include:

Obtain the preset contour error E, the length L of the line segment NP, the connection point P of the line segment NP and the arc PM, the angle β between the line segment NP and the arc PM, the position of the center C, and the center of the arc to the arc and the line segment. The direction vector of the connection point.

In step S22, the determination condition of the feature interpolation point according to the foregoing is the angle bisector of the angle between the line segment NP and the tangent of the arc PM of the feature interpolation point Q and the distance QP from the point P is E and according to the pre- The contour error E, the angle β, and the length L of the line segment NP are set to determine the position of the feature interpolation point Q.

In step S23 and step S24:

1. First establish a Cartesian coordinate system. Preferably, the line segment NP is set on the x-axis, and the N point is set to coincide with the coordinate origin. In combination with step S22, the coordinates of the feature interpolation point Q in the established coordinate system are obtained. :

In other embodiments, the positions of the x-axis and the y-axis may be reversed, such as setting the line segment NP on the y-axis and setting the N point to coincide with the coordinate origin.

And then determining, according to the first smooth transition condition and the first trajectory, a partial parameter of a connection point of the first trajectory and the first gyro curve and a partial parameter of a connection point of the first gyro curve at the first trajectory and the first gyro curve:

From the line segment NP on the x-axis, it can be determined that the ordinate of the I point is 0;

The curvature and the tangential angle of the line segment NP are 0, and the curvature of the first convolution curve at the point I and the tangential angle are both 0 by the first smooth transition condition.

3. According to the first convolution curve, the partial parameters of the connection point of the first trajectory and the first gyro curve, the partial parameters of the first gyroscopic curve at the connection point of the first trajectory and the first gyro curve, and the feature interpolation are satisfied according to the general curve of the interpolation curve. The position of the point is taken into the general equation of the interpolation curve to obtain the equation (1):

The ordinate of the I point is 0, the curvature of the first convolution curve at the I point is 0 and the tangential angle are 0, and the coordinates of the interpolation feature point are taken into the general equation of the interpolation curve to obtain the equations (1)

4. determining, according to the second smooth transition condition and the second trajectory, a partial parameter of a connection point of the second trajectory and the second gyro curve and a partial parameter of a connection point of the second gyro curve at the second trajectory and the second gyro curve:

According to the established Cartesian coordinate system and the position of the obtained center of the circle, the center of the arc to the direction of the connection point of the arc and the line segment, the coordinates of the center C in the Cartesian coordinate system are (x _c , y _c ), C point to Direction vector of point P

Express the coordinates of point O as

5. According to the second convolution curve, the general equation of the interpolation curve satisfies the position of the feature interpolation point, the partial parameter of the connection point of the second trajectory and the second gyro curve, and the second gyro curve in the second trajectory and the second gyroscopic curve. Part of the parameters of the connection point and the third smooth transition condition are brought into the general equation of the interpolation curve to obtain the equations (2):

The curvature of the second convolution curve at the feature interpolation point Q is expressed as a third smooth transition condition as

k ₁ =c ₀ ·s ₀

The tangential angle of the second convolution curve at the feature interpolation point Q is expressed as a third smooth transition condition as

The coordinates of the O point, the coordinates of the feature interpolation point Q, and the curvature and tangential angle of the second rotation curve at the feature interpolation point Q are brought into the general equation of the interpolation curve to obtain the equations (2)

Solving T _ccw , x ₀ , c ₀ , s ₀ , c ₁ , s ₁ and α according to equations (1) and (2); then T _ccw , x ₀ , c ₀ , s ₀ , c ₁ , s ₁ And α is brought back to the general equation of the interpolation curve to obtain the equations of the first convoluted curve and the second convoluted curve.

Referring to FIG. 4, FIG. 4 is a flowchart of a method for smoothing a motion track according to a third embodiment of the present invention. In this embodiment, preferably, the step of connecting the interpolation curves connecting the first trajectory, the feature interpolation point, and the second trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation The method includes: fitting a first sub-curve connecting the first trajectory and the feature interpolation point, and a second sub-curve connecting the feature interpolation point and the second trajectory, wherein the first sub-curve and the second sub-curve respectively satisfy the interpolation The general equation of the curve has different curve parameters, and the curvatures and tangential angles of the first sub-curve and the second sub-curve at the feature interpolation point are equal. The curvature of the first trajectory and the interpolation curve at the joint point of the two trajectories is equal to the tangential angle, and the curvature of the second trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle. The first sub-curve is a first clothoid curve and the second sub-curve is a second convoluted curve. The first track is a first arc and the second track is a second arc.

In this embodiment, the method for smoothing the motion track includes the following steps:

Step S31: Acquire a first arc and a second arc to be smoothly transferred and a preset contour error.

In step S31, for example, acquiring the first arc and the second arc to be smoothly transferred includes: acquiring the first arc and the second arc And the positional relationship between the two.

Step S32: determining the position of the feature interpolation point according to the first arc and the second arc and the preset contour error.

In step S32, for example, the determination condition of the feature interpolation point is that the first arc and the second arc are selected from the first arc and the second on the angle bisector of the angle of the tangent at the joint point of the two arcs. The point where the arc's connection point is the preset contour error is used as the feature interpolation point.

Step S33: setting a tangent line of the first arc at one end not connected to the second arc on a coordinate axis of the Cartesian coordinate system, and setting one end of the first arc not connected to the second arc to a right angle The origin of the coordinate system coincides.

Step S34: Solving the curve parameters of the first convoluted curve and the second convoluted curve by combining the following two sets of equations:

Where c ₀ is the curvature change rate of the first convoluted curve, and s ₀ is the arc length between the connection point of the first convoluted curve from the first arc to the feature interpolation point, and c ₁ is the second convoluted curve Curvature change rate, s ₁ is the arc length of the second convolution curve from the feature interpolation point to the connection point with the second arc, and α ₁ is the first arc from the first arc and the second arc The central angle of the arc segment between one end of the connection to the junction point of the first arc and the first convoluted curve, and α ₂ is the second arc from the connection point of the second convoluted curve to the second arc to the second circle The central angle of the arc segment between the arc and the connection point of the first arc, T _cw is the rotation matrix; c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw are required to pass through two groups The unknown amount that the equation solves;

Where R ₁ is the radius of the first arc, R ₂ is the radius of the second arc, E is the preset contour error, and x ₁ is the coordinate of the connection point of the first arc and the second arc on the coordinate axis. , y ₁ is the coordinate of the connection point of the first arc and the second arc on another coordinate axis of the Cartesian coordinate system, x ₂ is the coordinate of the center of the second arc on the coordinate axis, and y ₂ is the second circle The coordinates of the center of the arc on another coordinate axis of the Cartesian coordinate system,

a tangent vector of the first arc at the junction of the first arc and the second arc,

a tangent vector of the second arc at the junction of the first arc and the second arc,

a direction vector of a point connecting the center of the second arc to the first arc and the second arc, R ₁ , R ₂ , x ₁ , y ₁ , x ₂ , y ₂ ,

A known amount that can be obtained by the relative positional relationship of the first arc and the second arc and the two.

The derivation process is explained below.

First, the general equation of the interpolation curve is the same as that of the second embodiment, and the three expressions are respectively equations (3), (4), and (5), and are not described herein again.

Next, the determination of the position of the feature interpolation point and the derivation of the equations (6) and equations (7) are further explained.

It is noted that the curvature and the tangential angle of the first trajectory and the interpolation curve are equal to the first smooth transition condition, and the curvature and tangential angle of the second trajectory and the interpolation curve at the joint point are equal to the second Smoothing the transition condition, the curvatures and tangential angles of the first sub-curve and the second sub-curve at the feature interpolation point are equal to a third smooth transition condition.

Please refer to FIG. 5. FIG. 5 is a geometrical diagram of the first arc, the second arc, and the interpolation curve.

The smooth transition of the first circular arc NP and the second circular arc PM will be described below as an example. The I point is the connection point of the first arc NP and the first convolution curve IQ, the O point is the connection point of the second arc PM and the second convolution curve QO, and the P point is the first arc NP and the second arc PM Connection point. For the direction of motion, take N point → I point → Q point → O point → M point as an example.

It is assumed that the central angle corresponding to the arc segment NI on the first circular arc NP is α ₁ , and the central angle corresponding to the circular arc segment PO on the second circular arc PM is α ₂ .

In step S31, acquiring the first arc and the second arc to be smoothly transferred and the preset contour error include:

Obtaining the preset contour error E, the connection point P of the first arc NP and the second arc PM, and the tangent vector of the first arc NP at the connection point P

Tangent vector of the second arc PM at the joint point P

The angle β between the first circular arc NP and the second circular arc PM, the center O _{2 of the} second circular arc PM, and the direction vector of the center O ₂ to the connection point P

NP first arc radius R _1, the radius of the second arc PM R _2.

In step S32, the determination condition of the feature interpolation point according to the foregoing is the angle bisector of the angle of the feature interpolation point Q between the first arc NP and the tangent of the second arc PM and the distance from the P point QP. E and according to the preset contour error E, the angle β,

The position of the feature interpolation point Q is determined.

In step S33 and step S34:

1. First establish a Cartesian coordinate system. Preferably, the tangent of the first arc NP is set on the x-axis, and the N point is set to coincide with the coordinate origin. In combination with step S32, a right angle is established at the acquired P point. In the case where the coordinates in the coordinate system are (x ₁ , y ₁ ), the coordinates of the feature interpolation point Q in the established coordinate system are:

In other embodiments, the settings of the x-axis and the y-axis of the Cartesian coordinate system may be reversed, for example, setting a tangent to the first arc NP on the y-axis and setting the N point to coincide with the coordinate origin.

And then determining, according to the first smooth transition condition and the first trajectory, a partial parameter of a connection point of the first trajectory and the first gyro curve and a partial parameter of a connection point of the first gyro curve at the first trajectory and the first gyro curve:

From the radius R ₁ of the first arc NP, it can be known that all points on the first arc NP have curvatures

Determining, by the first smooth transition condition, that the curvature of the first convoluted curve at point I is equal to the curvature of the first arc NP at point I

Determining, by the first smooth transition condition, that the tangential angle of the first convoluted curve at point I is equal to the tangential angle of the first arc NP at point I. The tangential angle of the first convoluted curve at point I is expressed as

θ ₀ =α ₁

The coordinate of the I point is represented by the geometric relationship as

(R ₁ sinα ₁ , R ₁ (1-cosα ₁ ))

3. According to the first convolution curve, the partial parameters of the connection point of the first trajectory and the first gyro curve, the partial parameters of the first gyroscopic curve at the connection point of the first trajectory and the first gyro curve, and the feature interpolation are satisfied according to the general curve of the interpolation curve. The position of the point is taken into the general equation of the interpolation curve to obtain the equations (6):

The curvature and tangential angle of the first convolution curve at point I, the coordinates of point I, and the coordinates of the feature interpolation point are brought into the general equation of the interpolation curve to obtain the equations (6).

4. determining, according to the second smooth transition condition and the second trajectory, a partial parameter of a connection point of the second trajectory and the second gyro curve and a partial parameter of a connection point of the second gyro curve at the second trajectory and the second gyro curve:

The tangential angle of the second convolution curve at point O is expressed as a second smooth transition condition as

Among them, atan2 represents the inverse tangent function, but the angle is limited to the [-π, π] interval;

Express the curvature of the second convolution curve at point O as

Referring to step S32, it can be concluded that the coordinates of the point O are expressed as the coordinates of the acquired center O ₂ in the established Cartesian coordinate system (x ₂ , y ₂ ).

Where T is the rotation matrix

5. According to the second convolution curve, the general equation of the interpolation curve satisfies the position of the feature interpolation point, the partial parameter of the connection point of the second trajectory and the second gyro curve, and the second gyro curve in the second trajectory and the second gyroscopic curve. Part of the parameters of the connection point and the third smooth transition condition are brought into the general equation of the interpolation curve to obtain the equations (7):

The curvature of the second convolution curve at the feature interpolation point Q is expressed as a third smooth transition condition as

k ₁ =k ₀ +c ₀ ·s ₀

which is

The tangential angle of the second convolution curve at the feature interpolation point Q is expressed as a third smooth transition condition as

which is

The coordinates of the point O, the coordinates of the feature interpolation point Q, and the curvature and tangential angle of the second rotation curve at the feature interpolation point Q are brought into the general equation of the interpolation curve to obtain the equations (7).

Solving c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw according to equations (6) and (7), and then c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw brings back the general equation of the interpolation curve to obtain the equations of the first convoluted curve and the second convoluted curve.

Please refer to FIG. 6. FIG. 6 is a schematic block diagram of a motion track smoothing device according to an embodiment of the present invention. In this embodiment, the apparatus for smooth transition of the motion track comprises a processor 61 and a memory 62 connected to the processor 61. The memory 62 is configured to store the first track, the second track, and the preset contour error, and the processor 61 is used by the processor 61. Acquiring a first trajectory, a second trajectory, and a preset contour error from the memory; determining a position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error; according to the first trajectory, the second trajectory, and the feature interpolation point And the interpolation curve general equation fits the interpolation curve connecting the first trajectory, the feature interpolation point and the second trajectory, so that the curvature and the tangential angle of the first trajectory and the interpolation curve at the joint point of the two are equal, The curvature of the two trajectories and the interpolation curve are equal to the tangential angle at the joint point of the two.

For the foregoing steps of the processor 61, refer to the description in the method for smoothing the motion track according to any of the foregoing embodiments, and details are not described herein again.

Please refer to FIG. 7. FIG. 7 is a schematic diagram of a storage device according to an embodiment of the present invention. In the present embodiment, the storage device 70 stores program data, and the program data can be executed to implement the method of smooth transition of the motion trajectory described in any of the above embodiments.

The storage device 70 can be a USB flash drive, an optical disk, a hard disk, a mobile hard disk, a server, etc. Of course, the storage device can also be the memory 62 in the above embodiment.

Please refer to FIG. 8. FIG. 8 is a block diagram of a robot system according to an embodiment of the present invention. In this embodiment, the robot system includes a robot 81 and a controller 82 connected to the robot 81. The controller 82 is configured to acquire a first trajectory and a second trajectory to be smoothly transferred and a preset contour error; according to the first trajectory and The second trajectory and the preset contour error determine the position of the feature interpolation point; and the first trajectory, the feature interpolation point, and the second are fitted according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation The interpolation curve of the trajectory is such that the curvature of the first trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle, and the curvature of the second trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle.

In one embodiment, the robot 81 can include a tool, and the controller 82 can control the robot and the tool motion to perform processing, transportation, and the like according to the first trajectory, the second trajectory, and the interpolation curve. In another embodiment, the controller 82 can control the movement of the robot 81 based on the first trajectory, the second trajectory, and the interpolation curve.

For the foregoing steps of the controller 82, refer to the description in the method for smoothing the motion track according to any of the foregoing embodiments, and details are not described herein again.

Please refer to FIG. 9. FIG. 9 is a schematic diagram of a module of a numerical control machine tool according to an embodiment of the present invention. In this embodiment, the numerical control machine tool includes a machine tool body 91 and a numerical control device 92 connected to the machine tool body 91. The numerical control device 92 is configured to acquire a first track and a second track to be smoothly transferred and a preset contour error; The trajectory and the second trajectory and the preset contour error determine the position of the feature interpolation point; and the first trajectory, the feature interpolation point, and the first trajectory are fitted according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation The interpolation curve of the second trajectory is such that the curvature of the first trajectory and the interpolation curve at the joint point of the two trajectories is equal to the tangential angle, and the curvature of the second trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle.

In one embodiment, the machine tool body 91 can include a machining tool, and the numerical control device 92 can control the operation of the machine tool and the machining tool movement according to the first trajectory, the second trajectory, and the interpolation curve.

For the foregoing steps of the numerical control device 92, refer to the description in the method for smoothing the motion track according to any of the foregoing embodiments, and details are not described herein again.

The invention determines the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error by using the contour error as a condition for fitting the interpolation curve, and then according to the first trajectory, the second trajectory, and the feature interpolation point. And the interpolation curve general equation fits the interpolation curve connecting the first trajectory, the feature interpolation point and the second trajectory, on the one hand, the interpolation curve can be accurately fitted, and the interpolation curve fitted on the other hand can be Ensure the contour error of the equipment during processing.

The above is only the embodiment of the present invention, and is not intended to limit the scope of the invention, and the equivalent structure or equivalent process transformations made by the description of the invention and the drawings are directly or indirectly applied to other related technologies. The fields are all included in the scope of patent protection of the present invention.

Claims (29)

1. A method for smooth transition of motion trajectory, characterized in that the method comprises:

   Obtaining a first track and a second track to be smoothly transferred and a preset contour error;

   Determining a position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error;

   And fitting, according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation, an interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory, The curvature of the first trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle, and the curvature of the second trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle.
2. The method according to claim 1, wherein the determining the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error comprises:

   Selecting, at the angle bisector of the tangent angle at the connection point of the first trajectory and the second trajectory, a point at which the connection point of the first trajectory and the second trajectory is the preset contour error Feature interpolation points.
3. The method according to claim 1, wherein the first trajectory is an arc or a line segment, and the second trajectory is an arc.
4. The method according to claim 1, wherein said fitting said first trajectory according to said first trajectory, said second trajectory, said feature interpolation point, and an interpolation curve general equation The steps of the feature interpolation point and the interpolation curve of the second track include:

   And fitting a first sub-curve connecting the first trajectory and the feature interpolation point and connecting a second sub-curve between the feature interpolation point and the second trajectory, the first sub-curve and The second sub-curves respectively satisfy the interpolation curve general equation and have different curve parameters, and the curvature and tangential angle of the first sub-curve and the second sub-curve at the feature interpolation point are both equal.
5. The method according to claim 4, wherein the first sub-curve is a first convoluted curve and the second sub-curve is a second convoluted curve.
6. The method according to claim 5, wherein the first trajectory is a line segment, and the second trajectory is an arc, wherein the first trajectory, the second trajectory, and the feature interpolation point are And the step of fitting the interpolation curve general equation to fit the interpolation curve connecting the first trajectory, the feature interpolation point and the second trajectory comprises:

   Setting the line segment on a coordinate axis of the Cartesian coordinate system, and an end of the line segment not connected to the arc is disposed to coincide with an origin of the Cartesian coordinate system;

   The curve parameters of the first convolution curve and the second convolution curve are solved by combining the following two sets of equations:

   

   

   Where x ₀ is the coordinate of the connection point of the first convoluted curve and the line segment on the coordinate axis, c ₀ is the curvature change rate of the first convoluted curve, and s ₀ is the first convoluted curve From the junction length of the line segment to the feature interpolation point, c ₁ is the curvature change rate of the second convolution curve, and s ₁ is the second convolution curve from the feature insertion An arc length between the point of attachment to the arc and the arc, wherein α is the connection of the arc from the connection point of the second convolution curve to the arc to the arc and the line segment The central angle of the arc segment between the points, T _ccw is the rotation matrix, T _ccw , x ₀ , c ₀ , s ₀ , c ₁ , s ₁ and α are unknown quantities that need to be solved by the two sets of equations;

   Where L is the length of the line segment, E is the preset contour error, β is the angle between the line segment and the arc, R is the radius of the arc, and x _c is the arc The coordinate of the center of the circle on the coordinate axis, y _c is the coordinate of the center of the arc on another coordinate axis of the Cartesian coordinate system,

   

   a direction vector of the center of the arc to the point of intersection of the arc and the line segment, x _c , y _c ,

   

   L, β, and R are known quantities that can be obtained by the relative positional relationship of the line segment and the arc and both.
7. The method according to claim 5, wherein the first trajectory is a first circular arc, and the second trajectory is a second circular arc, wherein the according to the first trajectory, the second trajectory, and the The step of fitting the feature interpolation point and the interpolation curve general equation to fit the interpolation curve connecting the first trajectory, the feature interpolation point and the second trajectory comprises:

   Setting a tangent line at the end of the first arc that is not connected to the second arc to a coordinate axis of the Cartesian coordinate system, and connecting the first arc to the second arc One end is set to coincide with the origin of the Cartesian coordinate system;

   The curve parameters of the first convolution curve and the second convolution curve are solved by combining the following two sets of equations:

   

   

   Wherein c ₀ is a curvature change rate of the first convoluted curve, and s ₀ is an arc length between a connection point of the first convoluted curve and the first arc, to the feature interpolation point, c ₁ is a curvature change rate of the second convolution curve, and s ₁ is an arc length between the feature interpolation point and a connection point thereof with the second arc, α ₁ a center of the arc segment between the end of the first arc from the end of the first arc not connected to the second arc to the junction of the first arc and the first convolution curve angle, α ₂ is the second circular arc of the arc between the connection point from the connection point of the second curve and the second arc swirling to said second arc and the first arc The central angle of the segment, T _cw is a rotation matrix; c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw are unknown quantities that need to be solved by the two sets of equations;

   Wherein R ₁ is the radius of the first arc, R ₂ is the radius of the second arc, E is the preset contour error, and x ₁ is the first arc and the second circle a coordinate of a connection point of the arc on the coordinate axis, and y ₁ is a coordinate of a connection point of the first circular arc and the second circular arc on another coordinate axis of the rectangular coordinate system, where x ₂ is a coordinate of a center of the second arc on the coordinate axis, and y ₂ is a coordinate of a center of the second arc on another coordinate axis of the Cartesian coordinate system,

   

   a tangent vector of the first arc at a junction of the first arc and the second arc,

   

   a tangent vector of a connection point of the second circular arc between the first circular arc and the second circular arc,

   

   a direction vector of a point connecting the center of the second arc to the first arc and the second arc, R ₁ , R ₂ , x ₁ , y ₁ , x ₂ , y ₂ ,

   

   A known amount that can be obtained by the relative positional relationship of the first circular arc and the second circular arc and the two.
8. A device for smooth transition of motion trajectory, characterized in that the device comprises a processor and a memory connected to the processor, the memory is used for storing a first trajectory, a second trajectory, a preset contour error, The processor is configured to acquire the first trajectory, the second trajectory, and the preset contour error from the memory; determine feature insertion according to the first trajectory and the second trajectory and the preset contour error Position of the complementary point; fitting the first trajectory, the feature interpolation point, and the second trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation Interpolating curve such that the curvature of the first trajectory and the interpolation curve at the joint point of the two is equal to the tangential angle, and the curvature of the second trajectory and the interpolation curve at the joint point of the two The tangential angles are equal.
9. The apparatus according to claim 8, wherein the determining, by the processor, the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error comprises:

   Selecting, at the angle bisector of the tangent angle at the connection point of the first trajectory and the second trajectory, a point at which the connection point of the first trajectory and the second trajectory is the preset contour error Feature interpolation points.
10. The apparatus according to claim 8, wherein said first trajectory is an arc or a line segment, and said second trajectory is an arc.
11. The apparatus according to claim 8, wherein said processor is based on said first trajectory, said second trajectory, The feature interpolation point and the interpolation curve general equation fit the interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory, including:

    And fitting a first sub-curve connecting the first trajectory and the feature interpolation point and connecting a second sub-curve between the feature interpolation point and the second trajectory, the first sub-curve and The second sub-curves respectively satisfy the interpolation curve general equation and have different curve parameters, and the curvature and tangential angle of the first sub-curve and the second sub-curve at the feature interpolation point are both equal.
12. The apparatus according to claim 11, wherein said first sub-curve is a first convoluted curve and said second sub-curve is a second convoluted curve.
13. The apparatus according to claim 12, wherein the first trajectory is a line segment, and when the second trajectory is an arc, the processor is based on the first trajectory, the second trajectory, and the feature interpolation point. And an interpolation curve general equation for fitting an interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory, including:

    Setting the line segment on a coordinate axis of the Cartesian coordinate system, and one end of the line segment not connected to the arc is set to coincide with the origin of the Cartesian coordinate system; and solving by solving the following two sets of equations Curve parameters of the first convoluted curve and the second convoluted curve:

    

    

    Where x ₀ is the coordinate of the connection point of the first convoluted curve and the line segment on the coordinate axis, c ₀ is the curvature change rate of the first convoluted curve, and s ₀ is the first convoluted curve From the junction length of the line segment to the feature interpolation point, c ₁ is the curvature change rate of the second convolution curve, and s ₁ is the second convolution curve from the feature insertion An arc length between the point of attachment to the arc and the arc, wherein α is the connection of the arc from the connection point of the second convolution curve to the arc to the arc and the line segment The central angle of the arc segment between the points, T _ccw is the rotation matrix, T _ccw , x ₀ , c ₀ , s ₀ , c ₁ , s ₁ and α are unknown quantities that need to be solved by the two sets of equations;

    Where L is the length of the line segment, E is the preset contour error, β is the angle between the line segment and the arc, R is the radius of the arc, and x _c is the arc The coordinate of the center of the circle on the coordinate axis, y _c is the coordinate of the center of the arc on another coordinate axis of the Cartesian coordinate system,

    

    a direction vector of the center of the arc to the point of intersection of the arc and the line segment, x _c , y _c ,

    

    L, β, and R are known quantities that can be obtained by the relative positional relationship of the line segment and the arc and both.
14. The device according to claim 12, wherein the first track is a first arc and the second track is a second arc, the processor is according to the first track and the second track The feature interpolation point and the interpolation curve general equation fit the interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory, including:

    Setting a tangent line at the end of the first arc that is not connected to the second arc to a coordinate axis of the Cartesian coordinate system, and connecting the first arc to the second arc One end is set to coincide with the origin of the Cartesian coordinate system; the curve parameters of the first convolution curve and the second convolution curve are solved by combining the following two sets of equations:

    

    

    Wherein c ₀ is a curvature change rate of the first convoluted curve, and s ₀ is an arc length between a connection point of the first convoluted curve and the first arc, to the feature interpolation point, c ₁ is a curvature change rate of the second convolution curve, and s ₁ is an arc length between the feature interpolation point and a connection point thereof with the second arc, α ₁ a center of the arc segment between the end of the first arc from the end of the first arc not connected to the second arc to the junction of the first arc and the first convolution curve angle, α ₂ is the second circular arc of the arc between the connection point from the connection point of the second curve and the second arc swirling to said second arc and the first arc The central angle of the segment, T _cw is a rotation matrix; c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw are unknown quantities that need to be solved by the two sets of equations;

    Wherein R ₁ is the radius of the first arc, R ₂ is the radius of the second arc, E is the preset contour error, and x ₁ is the first arc and the second circle a coordinate of a connection point of the arc on the coordinate axis, and y ₁ is a coordinate of a connection point of the first circular arc and the second circular arc on another coordinate axis of the rectangular coordinate system, where x ₂ is a coordinate of a center of the second arc on the coordinate axis, and y ₂ is a coordinate of a center of the second arc on another coordinate axis of the Cartesian coordinate system,

    

    a tangent vector of the first arc at a junction of the first arc and the second arc,

    

    a tangent vector of a connection point of the second circular arc between the first circular arc and the second circular arc,

    

    a direction vector of a point connecting the center of the second arc to the first arc and the second arc, R ₁ , R ₂ , x ₁ , y ₁ , x ₂ , y ₂ ,

    

    A known amount that can be obtained by the relative positional relationship of the first circular arc and the second circular arc and the two.
15. A storage device, characterized in that the storage device stores program data, the program data being executable to implement the method of any of claims 1-7.
16. A robot system, comprising: a robot and a controller connected to the robot, the controller for acquiring a first trajectory and a second trajectory to be smoothly transferred and a preset contour error; Determining a position of the feature interpolation point by the first trajectory and the second trajectory and the preset contour error; and formulating a general equation according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve Interpolating an interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory such that a curvature and a tangential angle of the first trajectory and the interpolation curve at a joint point thereof Equally, the curvature of the second trajectory and the interpolation curve at the joint point of the two are equal to the tangential angle.
17. The robot system according to claim 16, wherein the controller determines the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error, including:

    Selecting, at the angle bisector of the tangent angle at the connection point of the first trajectory and the second trajectory, a point at which the connection point of the first trajectory and the second trajectory is the preset contour error Feature interpolation points.
18. The robot system according to claim 16, wherein the first trajectory is an arc or a line segment, and the second trajectory is an arc.
19. The robot system according to claim 16, wherein the controller fits the first trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and an interpolation curve general equation The interpolation curve of the feature interpolation point and the second track includes:

    And fitting a first sub-curve connecting the first trajectory and the feature interpolation point and connecting a second sub-curve between the feature interpolation point and the second trajectory, the first sub-curve and The second sub-curves respectively satisfy the interpolation curve general equation and have different curve parameters, and the curvature and tangential angle of the first sub-curve and the second sub-curve at the feature interpolation point are both equal.
20. The robot system according to claim 19, wherein the first sub-curve is a first convoluted curve and the second sub-curve is a second convoluted curve.
21. The robot system according to claim 20, wherein the first trajectory is a line segment, and when the second trajectory is an arc, the controller interpolates according to the first trajectory, the second trajectory, and the feature Point and interpolation curve general equations fit the interpolation curves connecting the first trajectory, the feature interpolation point and the second trajectory, including:

    Setting the line segment on a coordinate axis of the Cartesian coordinate system, and one end of the line segment not connected to the arc is set to coincide with the origin of the Cartesian coordinate system; and solving by solving the following two sets of equations Curve parameters of the first convoluted curve and the second convoluted curve:

    

    

    Where x ₀ is the coordinate of the connection point of the first convoluted curve and the line segment on the coordinate axis, c ₀ is the curvature change rate of the first convoluted curve, and s ₀ is the first convoluted curve From the junction length of the line segment to the feature interpolation point, c ₁ is the curvature change rate of the second convolution curve, and s ₁ is the second convolution curve from the feature insertion An arc length between the point of attachment to the arc and the arc, wherein α is the connection of the arc from the connection point of the second convolution curve to the arc to the arc and the line segment The central angle of the arc segment between the points, T _ccw is the rotation matrix, T _ccw , x ₀ , c ₀ , s ₀ , c ₁ , s ₁ and α are unknown quantities that need to be solved by the two sets of equations;

    Where L is the length of the line segment, E is the preset contour error, β is the angle between the line segment and the arc, R is the radius of the arc, and x _c is the arc The coordinate of the center of the circle on the coordinate axis, y _c is the coordinate of the center of the arc on another coordinate axis of the Cartesian coordinate system,

    

    a direction vector of the center of the arc to the point of intersection of the arc and the line segment, x _c , y _c ,

    

    L, β, and R are known quantities that can be obtained by the relative positional relationship of the line segment and the arc and both.
22. The robot system according to claim 20, wherein the first trajectory is a first arc and the second trajectory is a second arc, the controller is according to the first trajectory and the second The trajectory, the feature interpolation point, and the interpolation curve general equation fit the interpolation curve connecting the first trajectory, the feature interpolation point, and the second trajectory, including:

    Setting a tangent line at the end of the first arc that is not connected to the second arc to a coordinate axis of the Cartesian coordinate system, and connecting the first arc to the second arc One end is set to coincide with the origin of the Cartesian coordinate system; the curve parameters of the first convolution curve and the second convolution curve are solved by combining the following two sets of equations:

    

    

    Wherein c ₀ is a curvature change rate of the first convoluted curve, and s ₀ is an arc length between a connection point of the first convoluted curve and the first arc, to the feature interpolation point, c ₁ is a curvature change rate of the second convolution curve, and s ₁ is an arc length between the feature interpolation point and a connection point thereof with the second arc, α ₁ a center of the arc segment between the end of the first arc from the end of the first arc not connected to the second arc to the junction of the first arc and the first convolution curve angle, α ₂ is the second circular arc of the arc between the connection point from the connection point of the second curve and the second arc swirling to said second arc and the first arc The central angle of the segment, T _cw is a rotation matrix; c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw are unknown quantities that need to be solved by the two sets of equations;

    Wherein R ₁ is the radius of the first arc, R ₂ is the radius of the second arc, E is the preset contour error, and x ₁ is the first arc and the second circle a coordinate of a connection point of the arc on the coordinate axis, and y ₁ is a coordinate of a connection point of the first circular arc and the second circular arc on another coordinate axis of the rectangular coordinate system, where x ₂ is a coordinate of a center of the second arc on the coordinate axis, and y ₂ is a coordinate of a center of the second arc on another coordinate axis of the Cartesian coordinate system,

    

    a tangent vector of the first arc at a junction of the first arc and the second arc,

    

    a tangent vector of a connection point of the second circular arc between the first circular arc and the second circular arc,

    

    a direction vector of a point connecting the center of the second arc to the first arc and the second arc, R ₁ , R ₂ , x ₁ , y ₁ , x ₂ , y ₂ ,

    

    A known amount that can be obtained by the relative positional relationship of the first circular arc and the second circular arc and the two.
23. A numerical control machine tool, comprising: a machine tool body and a numerical control device, wherein the numerical control device is configured to acquire a first track and a second track to be smoothly transferred and a preset contour error; according to the first The trajectory and the second trajectory and the preset contour error determine a position of the feature interpolation point; and the connection is fitted according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation An interpolation curve of the first trajectory, the feature interpolation point, and the second trajectory, such that a curvature and a tangential angle of the first trajectory and the interpolation curve at a joint point thereof are equal, The curvature of the second trajectory and the interpolation curve at the joint point of the two are equal to the tangential angle.
24. The numerical control machine tool according to claim 23, wherein the determining, by the numerical control device, the position of the feature interpolation point according to the first trajectory and the second trajectory and the preset contour error comprises:

    Selecting, at the angle bisector of the tangent angle at the connection point of the first trajectory and the second trajectory, a point at which the connection point of the first trajectory and the second trajectory is the preset contour error Feature interpolation points.
25. The numerical control machine tool according to claim 23, wherein the first trajectory is an arc or a line segment, and the second trajectory is an arc.
26. The numerical control machine tool according to claim 23, wherein the numerical control device fits the first trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation The feature interpolation point and the first The interpolation curve of the two tracks includes:

    And fitting a first sub-curve connecting the first trajectory and the feature interpolation point and connecting a second sub-curve between the feature interpolation point and the second trajectory, the first sub-curve and The second sub-curves respectively satisfy the interpolation curve general equation and have different curve parameters, and the curvature and tangential angle of the first sub-curve and the second sub-curve at the feature interpolation point are both equal.
27. The numerical control machine tool according to claim 26, wherein the first sub-curve is a first convoluted curve and the second sub-curve is a second convoluted curve.
28. The numerical control machine tool according to claim 27, wherein the first trajectory is a line segment, and when the second trajectory is an arc, the numerical control device interpolates according to the first trajectory, the second trajectory, and the feature Point and interpolation curve general equations fit the interpolation curves connecting the first trajectory, the feature interpolation point and the second trajectory, including:

    Setting the line segment on a coordinate axis of the Cartesian coordinate system, and one end of the line segment not connected to the arc is set to coincide with the origin of the Cartesian coordinate system; and solving by solving the following two sets of equations Curve parameters of the first convoluted curve and the second convoluted curve:

    

    

    Where x ₀ is the coordinate of the connection point of the first convoluted curve and the line segment on the coordinate axis, c ₀ is the curvature change rate of the first convoluted curve, and s ₀ is the first convoluted curve From the junction length of the line segment to the feature interpolation point, c ₁ is the curvature change rate of the second convolution curve, and s ₁ is the second convolution curve from the feature insertion An arc length between the point of attachment to the arc and the arc, wherein α is the connection of the arc from the connection point of the second convolution curve to the arc to the arc and the line segment The central angle of the arc segment between the points, T _ccw is the rotation matrix, T _ccw , x ₀ , c ₀ , s ₀ , c ₁ , s ₁ and α are unknown quantities that need to be solved by the two sets of equations;

    Where L is the length of the line segment, E is the preset contour error, β is the angle between the line segment and the arc, R is the radius of the arc, and x _c is the arc The coordinate of the center of the circle on the coordinate axis, y _c is the coordinate of the center of the arc on another coordinate axis of the Cartesian coordinate system,

    

    a direction vector of the center of the arc to the point of intersection of the arc and the line segment, x _c , y _c ,

    

    L, β, and R are known quantities that can be obtained by the relative positional relationship of the line segment and the arc and both.
29. The numerical control machine tool according to claim 27, wherein said first track is a first arc and said second rail When the track is the second arc, the numerical control device fits the first trajectory according to the first trajectory, the second trajectory, the feature interpolation point, and the interpolation curve general equation, and the feature interpolation The interpolation curve of the point and the second trajectory includes:

    Setting a tangent line at the end of the first arc that is not connected to the second arc to a coordinate axis of the Cartesian coordinate system, and connecting the first arc to the second arc One end is set to coincide with the origin of the Cartesian coordinate system; the curve parameters of the first convolution curve and the second convolution curve are solved by combining the following two sets of equations:

    

    

    Wherein c ₀ is a curvature change rate of the first convoluted curve, and s ₀ is an arc length between a connection point of the first convoluted curve and the first arc, to the feature interpolation point, c ₁ is a curvature change rate of the second convolution curve, and s ₁ is an arc length between the feature interpolation point and a connection point thereof with the second arc, α ₁ a center of the arc segment between the end of the first arc from the end of the first arc not connected to the second arc to the junction of the first arc and the first convolution curve angle, α ₂ is the second circular arc of the arc between the connection point from the connection point of the second curve and the second arc swirling to said second arc and the first arc The central angle of the segment, T _cw is a rotation matrix; c ₀ , s ₀ , c ₁ , s ₁ , α ₁ , α ₂ , T _cw are unknown quantities that need to be solved by the two sets of equations;

    Wherein R ₁ is the radius of the first arc, R ₂ is the radius of the second arc, E is the preset contour error, and x ₁ is the first arc and the second circle a coordinate of a connection point of the arc on the coordinate axis, and y ₁ is a coordinate of a connection point of the first circular arc and the second circular arc on another coordinate axis of the rectangular coordinate system, where x ₂ is a coordinate of a center of the second arc on the coordinate axis, and y ₂ is a coordinate of a center of the second arc on another coordinate axis of the Cartesian coordinate system,

    

    a tangent vector of the first arc at a junction of the first arc and the second arc,

    

    a tangent vector of a connection point of the second circular arc between the first circular arc and the second circular arc,

    

    a direction vector of a point connecting the center of the second arc to the first arc and the second arc, R ₁ , R ₂ , x ₁ , y ₁ , x ₂ , y ₂ ,

    

    A known amount that can be obtained by the relative positional relationship of the first circular arc and the second circular arc and the two.

Images