CN110450150B

CN110450150B - Trajectory tracking control method and trajectory tracking system

Info

Publication number: CN110450150B
Application number: CN201810424413.0A
Authority: CN
Inventors: 吕伟新; 熊斌; 朱杰; 郭振杰; 王磊
Original assignee: Suzhou Ruiniu Robot Technology Co ltd
Current assignee: Suzhou Ruiniu Robot Technology Co ltd
Priority date: 2018-05-07
Filing date: 2018-05-07
Publication date: 2022-10-21
Anticipated expiration: 2038-05-07
Also published as: CN110450150A

Abstract

The invention discloses four trajectory tracking control methods and corresponding trajectory tracking systems, wherein the four trajectory tracking control methods have the same basic idea, and are characterized in that trajectory information described by a changed sensor coordinate system obtained by a sensor is firstly fixed, namely, the trajectory information is converted into a coordinate system description form suitable for integrally recording the trajectory information after detection and is marked, a real trajectory is gradually presented, a basis is provided for subsequently solving a target, an ideal state which should be presented by a tool at the current moment is found according to the target or a state which is reached when the next correction moment comes after the current correction is completed is found, and then an actuator is driven according to the information to correct the trajectory of the tool. The four track tracking control methods and the corresponding track tracking systems overcome errors caused by the advance of detection points, can be independent of teaching information, can correct position deviation and attitude deviation of a tool, and have good track tracking effect.

Description

Trajectory tracking control method and trajectory tracking system

Technical Field

The invention relates to the technical field of automatic trajectory tracking, in particular to a trajectory tracking control method and a trajectory tracking system for realizing the trajectory tracking control method.

Background

Operations such as welding, gluing, cutting and the like are similar conventional operations, and the common characteristic of the operations is that the operations are carried out along a specific track, and if the specific track position can be detected by a sensor, an actuator such as a robot can be used for tracking the track, so that automatic operation is realized.

Advanced ones of existing trajectory tracking systems use sensors to track the trajectory, for example arc welding robot systems use structured light vision sensors to track the trajectory. The actuator in the track tracking system has a task track preset by means of teaching information or planning and the like, the actuator with the tool can generate driving information according to the preset task track to enable the tool to run along the preset task track, and the track tracking system can only simply feed back track deviation information collected and extracted by a sensor to the actuator directly to correct the preset task track. The sensor is generally arranged at the front end of the actuator, so that the detection position of the sensor is inconsistent with the position of the tool in the track tracking process, the existing sensor for track tracking can only provide the detected track deviation information relative to the existing sensor for track tracking, the track tracking control method adopted in the track control system ignores the problem that the detection position of the sensor is inconsistent with the position of the tool, and the position deviation detected by the sensor is roughly and directly used for correcting the position of the tool. The method is based on track deviation information detected by a sensor, and has no deviation and correction, so that even if the current position of a tool has no deviation and the tool position is deviated at the next moment after the tool advances according to a preset task track, the whole-course deviation-free tracking can not be realized theoretically, the deviation can only be reduced to be not more than a new deviation caused by each advance step, and therefore, the track tracking purpose can be well realized only under the condition that the curvature radius of the track is larger or the fluctuation of the track is smaller, but a larger track tracking error can be generated when the track tracking is performed under the condition that the curvature radius of the track is smaller or the fluctuation of the track is larger, and even the track tracking purpose can not be finished. At present, the construction of a tracking system is complicated, and a general track tracking control method and a track tracking system for realizing the track tracking control method are unavailable for tracks, particularly space curve tracks.

Disclosure of Invention

The operations of welding, gluing, cutting and the like are similar conventional operations, and the common characteristics of the operations are that the operations are carried out along a specific track, the track is a line segment which is bent at will and has a posture, and is a target tracked when a tool works in an ideal state, the characteristic that the surface of a workpiece on which the track is located faces in a detection point is called the posture of the track, the characteristic that the workpiece on which the track is located faces in the detection point is called the posture of the track, the specific track can be abstracted into a straight line segment or a curved line segment, and the straight line segment or the curved line segment is the track to be tracked by a track tracking system and is called a track characteristic line.

The technical problems to be solved by the invention are as follows: the trajectory tracking control method and the trajectory tracking system are universal, can be operated independently without teaching information, can realize automatic tracking and have good tracking effect.

In order to solve the above problems, the first trajectory tracking method of the present invention is to utilize the characteristic of the sensor to detect the trajectory in advance, and to use a coordinate transformation method to mark the original position information of the trajectory detected by the sensor into the workpiece coordinate system or the world coordinate system, to form a trajectory position information set { Q } composed of a plurality of detection points, and then to determine the correct position and posture that the tool should reach at the next moment according to the trajectory position information set { Q }.

In the first trajectory tracking control method according to the present invention, the tool is attached to the actuator, the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a certain geometric relationship, specifically including the steps of:

(1) Detecting a track: acquiring original track information of a detection point on a track on a workpiece through a sensor;

(2) Extracting track information: extracting position information of detection points from the original track information, and expressing the position information by a sensor coordinate system;

(3) Marking track information: converting the position information expressed by the sensor coordinate system into position information expressed by a coordinate system attached to the track, marking the position information in the coordinate system attached to the track, and storing the position information in an information storage area to form a track position information set { Q }; when the position of the workpiece is not fixed, the coordinate system attached to the track is a workpiece coordinate system, and when the position of the workpiece is fixed, the coordinate system attached to the track is one of the workpiece coordinate system and a world coordinate system;

(4) Determining a target: and determining the target position of the tool at the next moment according to the track position information set { Q }, which is as follows: setting a target position of a tool at the current moment as a point P, wherein a tool coordinate system of the point P is P-X-tool ' Y-tool ' Z-tool ', a plane perpendicular to a track characteristic line is made through the point P, an intersection point of the plane and the track characteristic line is a reference point Ref-A, the reference point Ref-A is taken as a tracking target, and the position of the reference point Ref-A is taken as a target position of the tool at the next moment;

(5) Driving a tool: the actuator enables the target position of the tool to move from the target position at the current moment to a Ref-A point according to the position information of the reference point Ref-A, and moves forwards by a step delta = V t along the positive direction of the Y-tool' axis by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool;

(6) And circulating the steps until the tracking of the whole track is completed.

Further, in the first trajectory tracking control method, in step (4), the target position and the attitude of the tool at the next time are determined according to the trajectory position information set { Q }, which is specifically as follows: setting a target position of a tool at the current moment as a point P, setting a tool coordinate system of the point P as P-X-tool ' Y-tool ' Z-tool ', making a plane perpendicular to a track characteristic line through the point P, taking an intersection point of the plane and the track characteristic line as a reference point Ref-A, and determining the tool coordinate system P-X-tool Y-tool Z-tool at the reference point Ref-A by taking the reference point Ref-A as a tracking target: making a tangent line of the track characteristic line by passing through a reference point Ref-A, and taking the tangent line of the track characteristic line and a plane passing through a straight line parallel to the axis of the Z-tool' as a track segmentation plane; a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and the posture of the reference point Ref-A as the target position and the posture of the tool at the next moment; in the step (5), the actuator moves the target position of the tool from the target position at the current moment to the Ref-A point according to the position and posture information of the reference point Ref-A, and moves forward by a step delta = V × t along the positive direction of the Y-tool axis by taking Ref-A as a starting point.

Further, in the first trajectory tracking control method, in step (2), a vector in the same direction as the Z-tool' axis is used as the attitude information of the detection point, and the attitude information is expressed by using a sensor coordinate system.

In step (3), the attitude information expressed by the sensor coordinate system is converted into attitude information expressed by the coordinate system to which the track is attached, and then the attitude information is marked in the coordinate system to which the track is attached and is stored in the information storage area to form a track attitude information set { N }, and the track attitude information set and the track position information set jointly form a track position attitude information set { Q, N }.

In step (4), according to the track position and posture information set { Q, N }, determining a tool coordinate system P-X-toolY-toolZ-tool located at the reference point Ref-a: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersecting line of the plane and the track segmenting surface is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; and taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment.

In the step (5), the actuator moves the target position of the tool from the target position at the current moment to the Ref-A point according to the position and posture information of the reference point Ref-A, and moves forward by a step delta = V × t along the positive direction of the Y-tool axis by taking Ref-A as a starting point.

Further, in the foregoing first trajectory tracking control method, in step (4), the target position of the tool at the next time is determined according to the trajectory position information set { Q }, which is specifically as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the track characteristic line through a control reference point Ref-A-next, wherein the intersection line of the plane and the track division surface is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment; at this time, in the step (5), the actuator moves the target position of the tool from the target position at the current moment to the target position at the next moment according to the position and posture information of the control reference point Ref-A-next.

Further, in the first trajectory tracking control method, the conversion process of converting the position information expressed by the sensor coordinate system into the position information expressed by the coordinate system to which the trajectory is attached in step (3) includes: the position information expressed in the sensor coordinate system is first converted into the position information expressed in the tool coordinate system, and then the position information expressed in the tool coordinate system is converted into the position information expressed in the coordinate system to which the trajectory is attached.

The track tracking system for realizing the first track tracking control method comprises a sensor, a tool and an actuator, wherein the tool is attached to the actuator, the sensor is attached to a joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

the method comprises the steps that a sensor collects original track information of a track on a workpiece;

the information detection processor extracts the position information of the track according to the original track information acquired by the sensor and expresses the position information by a sensor coordinate system;

the positive kinematics module acquires joint angle information of an actuator when the target position of the tool is at the target position at the current moment, and calculates the target position and the posture information of the tool at the current moment according to the joint angle information of the actuator;

the information application processor converts the position information of the track expressed by the sensor coordinate system into position information expressed by a coordinate system attached to the track, then marks the position information in the coordinate system attached to the track and stores the position information in the information storage area to form a track position information set { Q };

the information application processor calls the target position and the attitude information of the tool at the current moment calculated in the positive kinematics module, and determines the target position and the attitude information of the tool at the next moment according to the target position and the attitude information of the tool at the current moment and the track position information set { Q };

the inverse kinematics module calls the target position and the attitude information of the tool calculated in the information application processor at the next moment, and calculates the joint angle information of the actuator when the target position of the tool is positioned at the target position at the next moment according to the target position and the attitude information of the tool at the next moment.

The second track tracking method of the invention is characterized in that the original position information of the track detected by the sensor is marked in a workpiece coordinate system or a world coordinate system by using a coordinate transformation method by utilizing the characteristic that the sensor detects the track in advance, a track position information set { Q } consisting of a plurality of detection points is formed, the correct position and posture which the tool should reach at the next moment are determined according to the track position information set { Q }, then the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment is further solved, the target position and posture of the tool at the current moment are changed according to the position and posture deviation information to reach the correct position posture at the next moment, and the tool falls on the correct position posture after each step of advance, thereby realizing the theoretical non-deviation tracking effect, and overcoming the error caused by the advance of the detection points.

In a second trajectory tracking method according to the present invention, a tool is attached to an actuator, a sensor is attached to a joint of the actuator or the sensor is directly attached to the tool, and a relative position between the sensor and the tool has a certain geometric relationship, and the method specifically includes the following steps:

(4) Determining a target: determining the target position and the attitude of the tool at the next moment according to a track position information set { Q }, and calculating the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment, wherein the position and attitude deviation information is lateral deviation delta x, or the lateral deviation delta x is combined with at least one of forward deviation delta y, height deviation delta z, pitching angle deviation delta x and forward direction angle deviation delta z; the specific determination method for determining the target position and the attitude of the tool at the next moment according to the trajectory position information set { Q } is as follows: setting a target position of a tool at the current moment as a point P, setting a tool coordinate system of the point P as P-X-tool ' Y-tool ' Z-tool ', making a plane perpendicular to a track characteristic line through the point P, taking an intersection point of the plane and the track characteristic line as a reference point Ref-A, and determining the tool coordinate system P-X-tool Y-tool Z-tool at the reference point Ref-A by taking the reference point Ref-A as a tracking target: making a tangent line of the track characteristic line by passing through a reference point Ref-A, and taking the tangent line of the track characteristic line and a plane passing through a straight line parallel to the axis of the Z-tool' as a track segmentation plane; a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment;

(5) A driving tool: the actuator enables the target position of the tool to move to a Ref-A point from the target position at the current moment according to the position and posture deviation information of the reference point Ref-A, and forwards moves by a step delta = V x t along a preset direction by taking Ref-A as a starting point, wherein t is the adjustment interval time of correction control of each position of the tool, and V is the advancing speed of the tool; and the corrected driving information for moving the target position of the tool from the target position at the current moment to the Ref-A point is driving information D-tool, and the driving information D-tool at least comprises one effective component: Δ x, and the driving information D-tool is a vector containing at most five significant components: vectors of Δ x, Δ y, Δ z, δ x, δ z; wherein the predetermined direction is a direction that is naturally determined due to the displacement adjustment when the drive information D-tool does not contain δ x or δ z; when the driving information D-tool comprises deltax or deltaz, the preset direction is the positive direction of the Y-tool axis;

Further, in the second trajectory tracking control method, the specific solving process of the position and posture deviation information in the step (4) is as follows: setting a tool coordinate system of a target position P point at the current moment as P-X-tool ' Y-tool ' Z-tool ',

δ z is: an included angle between the projection S-xy of a tangent line positioned at the reference point Ref-A on the track characteristic line in a plane passing through an X-tool ' axis and a Y-tool ' axis and the direction of the Y-tool ' axis;

δ x is: the included angle between the projection S-yz of a tangent line positioned at the reference point Ref-A on the track characteristic line in the plane passing through the Y-tool ' axis and the Z-tool ' axis and the Y-tool ' axis direction;

Δ x is: the projection length of the vector from the point P to the reference point Ref-A in the X-tool' axis direction;

Δ y is: the projection length of the vector from the point P to the reference point Ref-A in the Y-tool' axis direction;

Δ z is: the projection length of the vector from point P to reference point Ref-a in the direction of the Z-tool' axis.

Further, in the second trajectory tracking control method, the specific determination manner for determining the target position and posture of the tool at the next time according to the set { Q } of trajectory position information in step (4) is as follows: taking a reference point Ref-A as a sphere center, taking the radius of which is = V as a sphere, wherein the sphere has two intersection points with a track characteristic line, taking the intersection point of the sphere located in the advancing direction of the tool and the track characteristic line as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' of the tool located at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as an axis Y-tool, wherein the positive direction of the axis Y-tool points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment; and according to the position and attitude information of the control reference point Ref-A-next, calculating the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment, wherein the position and attitude deviation information is the combination of the lateral deviation delta x or the lateral deviation delta x and at least one of the advancing deviation delta y, the height deviation delta z, the pitching angle deviation delta x and the advancing direction angle deviation delta z. In the step (5), the actuator makes the target position of the tool from the target position at the current moment to the target position at the next moment according to the position and posture information of the control reference point Ref-A-next.

Further, in the second trajectory tracking control method, the specific solving process of the position and attitude deviation information in the step (4) is as follows: setting a tool coordinate system of a target position P point at the current moment as P-X-tool ' Y-tool ' Z-tool ',

δ z is: an included angle between the projection S-xy of a tangent line positioned at the control reference point Ref-A-next on the track characteristic line in the plane determined by the X-tool 'axis and the Y-tool' axis direction;

δ x is: an included angle between the projection S-yz of a tangent line positioned at the control reference point Ref-A-next on the track characteristic line in a plane determined by the axis of Y-tool ' and the axis of Z-tool ' and the direction of the axis of Y-tool ';

Δ x is: the projection length of the vector from the point P to the control reference point Ref-A-next in the X-tool' axis direction;

Δ y is: the projection length of the vector from the point P to the control reference point Ref-A-next in the Y-tool' axis direction;

Δ z is: the projection length of the vector from the point P to the control reference point Ref-A-next in the direction of the Z-tool' axis.

Further, in the second trajectory tracking control method, the modified driving information in step (5) is integrated driving information in which the driving information D-tool is combined with at least one of predetermined task trajectory information and intervention information.

Further, in the second trajectory tracking control method, the conversion process of converting the position information expressed by the sensor coordinate system into the position information expressed by the coordinate system to which the trajectory is attached in step (3) includes: the position information expressed in the sensor coordinate system is first converted into position information expressed in the tool coordinate system, and then the position information expressed in the tool coordinate system is converted into position information expressed in the coordinate system to which the trajectory is attached.

The track tracking system for realizing the second track tracking control method comprises a sensor, a tool and an actuator, wherein the tool is attached to the actuator, the sensor is attached to a joint of the actuator or directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

the positive kinematics module acquires the joint angle information of an actuator when the target position of the tool is at the current target position, and calculates the target position and the attitude information of the tool at the current time according to the joint angle information of the actuator;

the information application processor calls the target position and the attitude information of the tool at the current moment calculated in the positive kinematics module, determines the target position and the attitude information of the tool at the next moment according to the target position and the attitude information of the tool at the current moment and a track position information set { Q }, and calculates the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment;

the inverse kinematics module calls position and posture deviation information between a target position of the tool at the current moment and a target position of the tool at the next moment, which is calculated in the information application processor, and calculates joint angle information when the target position of the tool is located at the target position at the next moment according to the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment.

The trajectory is an arbitrarily curved line segment with a "posture" which is a target tracked by the tool when the tool works under an ideal state, wherein the "posture" means that not only a specific point on the tool is required to be on a trajectory characteristic line during trajectory tracking, but also an axis of the tool is required to form a certain angular relationship with the trajectory characteristic line. The sensors involved in the first trajectory tracking control method, the second trajectory tracking control method, and the corresponding trajectory tracking systems are capable of sensing only the deviation of the target trajectory in the direction away from the tool, the deviation in the directions on both sides of the trajectory, and the position information of the trajectory, and the sensor is not capable of sensing the characteristic of "which direction" the workpiece surface on which the trajectory is located is facing at the detection point, and the characteristic of "which direction" the workpiece surface on which the trajectory is located is facing at the detection point is referred to herein as the "posture" of the trajectory. The third trajectory tracking method and the sensor related in the corresponding trajectory tracking system can not only sense the deviation of the target trajectory in the direction away from the tool and the deviation in the directions of two sides of the trajectory, but also sense the attitude information of the trajectory. The third track tracking method of the invention is to use the characteristic that a sensor detects a track in advance, adopt a coordinate transformation method, mark the original position and attitude information of the track detected by the sensor into a workpiece coordinate system or a world coordinate system to form a track position and attitude information set { Q, N } which is composed of a plurality of detection points, and then determine the correct target position and attitude which a tool should reach at the next moment according to the track position and attitude information set { Q, N }.

In the third track tracking method, a sensor can sense position information and a posture of a track, a tool is attached to an actuator, the sensor is attached to a joint of the actuator or directly attached to the tool, and a relative position between the sensor and the tool has a determined geometric relationship, and the method specifically comprises the following steps:

(2) Extracting track information: extracting position information of a detection point and attitude information of a track segmentation surface at the detection point from the original track information, and expressing the position information and the attitude information by a sensor coordinate system;

(3) Marking track information: converting the position and posture information expressed by the sensor coordinate system into position and posture information expressed by a coordinate system attached to the track, marking the position and posture information in the coordinate system attached to the track, and storing the position and posture information in an information storage area to form a track position and posture information set { Q, N }; when the position of the workpiece is not fixed, the coordinate system attached to the track is a workpiece coordinate system, and when the position of the workpiece is fixed, the coordinate system attached to the track is one of the workpiece coordinate system or a world coordinate system;

(4) Determining a target: and determining the target position and the attitude of the tool at the next moment according to the track position and attitude information set { Q, N }, wherein the target position and the attitude of the tool at the next moment are as follows:

setting the target position of the tool at the current moment as a point P, making a plane perpendicular to a track characteristic line by passing the point P, taking the intersection point of the plane and the track characteristic line as a reference point Ref-A, taking the reference point Ref-A as a tracking target, and determining a tool coordinate system P-X-toolY-toolZ-tool positioned at the reference point Ref-A: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the reference point Ref-A as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment;

(5) Driving a tool: the actuator enables the target position of the tool to move from the target position of the current moment to a Ref-A point according to the position and posture information of a reference point Ref-A, and moves forwards by a step delta = V t along the positive direction of the Y-tool shaft by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool;

Further, in the third trajectory tracking control method, the specific determination method for determining the target position and the posture of the tool at the next time according to the set { Q, N } of the trajectory position and posture information in step (4) is as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by passing through a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; and taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment. At this time, in the step (5), the actuator moves the target position of the tool from the target position at the current time to the target position at the next time according to the position and posture information of the control reference point Ref-a-next.

Further, in the third trajectory tracking control method, the conversion process of converting the position and orientation information expressed by the sensor coordinate system into the position and orientation information expressed by the coordinate system to which the trajectory is attached in step (3) is: the position and attitude information expressed by the sensor coordinate system is first converted into the position and attitude information expressed by the tool coordinate system, and then the position and attitude information expressed by the tool coordinate system is converted into the position and attitude information expressed by the coordinate system to which the track is attached.

The trajectory tracking system for realizing the third trajectory tracking control method comprises a sensor, a tool and an actuator, wherein the tool is attached to the actuator, the sensor is attached to a joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

the information detection processor extracts the position and posture information of the track according to the original track information acquired by the sensor and expresses the position and posture information by a sensor coordinate system;

the information application processor converts the position and posture information of the track expressed by the sensor coordinate system into the position and posture information expressed by the coordinate system attached to the track, marks the position and posture information in the coordinate system attached to the track and stores the position and posture information in the information storage area to form a track position and posture information set { Q, N };

the information application processor calls the target position and the attitude information of the tool at the current moment calculated in the positive kinematics module, and determines the target position and the attitude of the tool at the next moment according to the target position and the attitude information of the tool at the current moment and the track position and attitude information set { Q, N };

the inverse kinematics module calls the target position and the attitude information of the tool calculated in the information application processor at the next moment, and calculates the joint angle information of the actuator when the target position of the tool is located at the target position at the next moment according to the target position and the attitude of the tool at the next moment.

The fourth trajectory tracking method and the corresponding sensor in the trajectory tracking system can not only sense the deviation of the target trajectory in the direction away from the tool and the deviation of the target trajectory in the directions of two sides of the trajectory, but also sense the attitude information of the trajectory. The fourth track tracking method of the invention is characterized in that the original position and attitude information of the track detected by the sensor are marked in a workpiece coordinate system or a world coordinate system by utilizing the characteristic that the sensor detects the track in advance, a coordinate transformation method is adopted, a track position and attitude information set { Q } consisting of a plurality of detection points is formed, the correct position and attitude which the tool should reach at the next moment are determined according to the track position and attitude information set { Q }, then the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment is further solved, the target position and attitude at the current moment of the tool are changed according to the position and attitude deviation information to enable the target position and attitude to reach the correct position and attitude at the next moment, and the tool falls on the correct position and attitude after each step of advance, the theoretical non-deviation tracking effect is realized, and the error caused by the advance of the detection points is overcome.

In the fourth trajectory tracking method according to the present invention, the sensor may sense position information and a posture of the trajectory, the tool is attached to the actuator, the sensor is attached to a joint of the actuator or the sensor is directly attached to the tool, and a relative position between the sensor and the tool has a certain geometric relationship, and specifically includes the following steps:

(4) Determining a target: determining a target position and a posture of the tool at the next moment according to a track position posture information set { Q, N }, and calculating position and posture deviation information between the target position posture of the tool at the current moment and a target of the tool at the next moment, wherein the position and posture deviation information is position deviation information which at least contains two effective components of delta x and delta y in six effective components of lateral deviation delta x, advancing deviation delta y, height deviation delta z, pitching angle deviation delta x, lateral deviation angle deviation delta y and advancing direction angle deviation delta z, and the position and posture deviation information is position deviation information which at most contains six effective components of delta x, delta y, delta z, delta x, delta y and delta z; the specific determination method for determining the target position and the attitude of the tool at the next moment according to the track position and attitude information set { Q, N } is as follows: setting the target position of the tool at the current moment as a point P, making a plane perpendicular to a track characteristic line by passing the point P, taking the intersection point of the plane and the track characteristic line as a reference point Ref-A, taking the reference point Ref-A as a tracking target, and determining a tool coordinate system P-X-toolY-toolZ-tool positioned at the reference point Ref-A: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersecting line of the plane and the track segmenting surface is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a reference point Ref-A as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment;

(5) Driving a tool: the actuator enables the target position of the tool to move from the target position at the current moment to the target position at the next moment according to the reference point Ref-A position and the attitude deviation information; the correction driving information for moving the target position of the tool from the current target position to the next target position is driving information D-tool, and the driving information D-tool at least comprises two effective components: Δ x, δ y, and the driving information D-tool is a vector containing at most six significant components: vectors of Δ x, Δ y, Δ z, δ x, δ y, δ z;

Further, in the fourth trajectory tracking control method, the specific solving process of the position and posture deviation information in the step (4) is as follows: setting a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' at a target position P point at the current moment,

Δ z is: the projection length of the vector from the point P to the reference point Ref-A in the Z-tool' axis direction;

δ x is: linearizing a track characteristic line positioned at a reference point Ref-A or drawing a tangent line of the track characteristic line through the reference point Ref-A, wherein the track characteristic line is linearized or the tangent line of the track characteristic line forms an included angle between a projection line segment S-yz and a Y-tool ' axis on a plane passing through the Y-tool ' axis and the Z-tool ' axis;

δ y is: the trajectory characteristic line at the reference point Ref-A is linearized or passes through the reference point Ref-A to be taken as a tangent line of the trajectory characteristic line, and the trajectory characteristic line is linearized or the tangent line of the trajectory characteristic line forms an included angle between a projection line segment S-xz and a Z-tool ' axis on a plane passing through the X-tool ' axis and the Z-tool ' axis;

δ z is: and (3) making the trajectory characteristic line at the reference point Ref-A into a straightness or a tangent line passing through the reference point Ref-A as the tangent line of the trajectory characteristic line, and making the trajectory characteristic line into the straightness or the included angle between the projection line segment S-xy of the tangent line of the trajectory characteristic line on a plane passing through the X-tool 'axis and the Y-tool' axis.

Further, in the fourth trajectory tracking control method, the specific determination method for determining the target position and the posture of the tool at the next time according to the set { Q, N } of the trajectory position and posture information in step (4) is as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the track characteristic line through a control reference point Ref-A-next, wherein the intersection line of the plane and the track division surface is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment, and calculating the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment according to the position and posture information of the control reference point Ref-A-next; wherein t is the adjustment interval time of each position correction control of the tool, V is the advancing speed of the tool, and the tool at the current time position moves to the target position at the next time after the interval time t. At this time, the actuator in step (5) moves the target position of the tool from the target position at the current time to the target position at the next time according to the position and posture information of the control reference point Ref-a-next.

Further, in the fourth trajectory tracking control method, the specific solving process of the position and attitude deviation information in the step (4) is as follows: setting the tool coordinate system of the target position P point of the tool at the current moment as P-X-tool ' Y-tool ' Z-tool ',

Δ z is: the projection length of the vector from the point P to the control reference point Ref-A-next in the Z-tool' axis direction;

δ x is: the trajectory characteristic line at the position of the control reference point Ref-A-next is linearized or passes through the control reference point Ref-A-next to be taken as a tangent line of the trajectory characteristic line, and the trajectory characteristic line is linearized or the tangent line of the trajectory characteristic line forms an included angle between a projection line segment S-yz and a Y-tool ' axis on a sectioning plane passing through the Y-tool ' axis and the Z-tool ' axis;

δ y is: the trajectory characteristic line at the position of the control reference point Ref-A-next is linearized or passes through the control reference point Ref-A-next to be taken as the tangent line of the trajectory characteristic line, and the trajectory characteristic line is linearized or the tangent line of the trajectory characteristic line forms an included angle between a projection line segment S-xz and a Z-tool ' axis on a sectioning plane passing through the X-tool ' axis and the Z-tool ' axis;

δ z is: and (3) making the trajectory characteristic line at the position of the control reference point Ref-A-next linear or passing through the control reference point Ref-A-next as a tangent of the trajectory characteristic line, and making an included angle between a projection line segment S-xy and a Y-tool ' axis of the trajectory characteristic line linear or the tangent of the trajectory characteristic line on a plane passing through an X-tool ' axis and a Y-tool ' axis.

Further, in the fourth trajectory tracking control method, the modified driving information in step (5) is integrated driving information in which the driving information D-tool is combined with at least one of predetermined task trajectory information and intervention information.

Further, in the fourth trajectory tracking control method, the conversion process of converting the position and orientation information expressed by the sensor coordinate system into the position and orientation information expressed by the coordinate system to which the trajectory is attached in step (3) includes: the position and posture information expressed by the sensor coordinate system is converted into the position and posture information expressed by the tool coordinate system, and then the position and posture information expressed by the tool coordinate system is converted into the position and posture information expressed by the coordinate system attached to the track.

The track tracking system for realizing the fourth track tracking control method comprises a sensor, a tool and an actuator, wherein the sensor can sense the position information and the posture of a track, the tool is attached to the actuator, the sensor is attached to a joint of the actuator or directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

a sensor collects original track information on a workpiece track;

the information application processor calls the target position and the attitude information of the tool at the current moment calculated in the positive kinematics module, determines the target position and the attitude of the tool at the next moment according to the target position and the attitude information of the tool at the current moment and the track position and attitude information set { Q, N }, and calculates the position and attitude deviation information between the position of the tool at the current moment and the target position of the tool at the next moment;

the inverse kinematics module calls the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment, which is calculated in the information application processor, and calculates the joint angle information of the actuator when the target position of the tool is located at the target position at the next moment according to the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment.

The trajectory tracking system for implementing the first trajectory tracking control method, the trajectory tracking system for implementing the second trajectory tracking control method, the trajectory tracking system for implementing the third trajectory tracking control method, and the trajectory tracking system for implementing the fourth trajectory tracking control method may be further divided into two parts, namely a sensing system and an execution system, and the specific division is as follows:

the first division mode: the sensor and the information detection processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the information application processor, the forward kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end. In the actual use process, the sensor and the information detection processor can be made into a whole, and the information detection processor can also be made into a single back-end information processor. In operation the sensing system downloads to the execution system positional information of the trajectory expressed in the sensor coordinate system.

The second division mode is as follows: the sensor, the information detection processor and the information application processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the positive kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system is electrically connected with the execution system through the first information interface end and the second information interface end. In the actual use process, the sensor, the information detection processor and the information application processor can be integrated, or the information detection processor and the information application processor can be made into a single back-end information processor. When the tool is in work, the sensing system receives target position and attitude information of the tool at the current moment, which is uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

The third division mode: the sensor, the information detection processor, the information application processor and the positive kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end. In the actual use process, the sensor, the information detection processor, the information application processor and the positive kinematics module can be made into a whole, or the information detection processor, the information application processor and the positive kinematics module can be made into a single back-end information processor. When the tool is in work, the sensing system receives the joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

The fourth division mode: the sensor, the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the actuator forms an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end. In the actual use process, the sensor, the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module can be made into a whole, or the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module can be made into a single back-end information processor. When the tool is in work, the sensing system receives the joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the joint angle information of the actuator when the tool reaches the target position and posture at the next moment to the execution system.

The invention has the beneficial effects that: the four track tracking control methods and the corresponding track tracking systems overcome errors caused by the advance of the detection points, can independently run without teaching information, can fully utilize information detected by the sensor to track in the track tracking process, can correct position deviation of a tool, and can correct attitude deviation of the tool, so that the aim of track tracking is better fulfilled. In addition, the track tracking system is further divided into a sensing system and an execution system, so that the tracking system can be established more conveniently and flexibly, and can be conveniently manufactured by different professional manufacturers, the performance is improved, and the cost is reduced.

Drawings

Fig. 1 is a schematic structural diagram of a sensor and a tool in the trajectory tracking control method and the trajectory tracking system according to the present invention.

Fig. 2 is a block diagram of information relationship between a first division manner of a sensing system and an execution system in the trajectory tracking system according to the present invention.

Fig. 3 is a block diagram of information relationship between a second division manner of the sensing system and the execution system in the trajectory tracking system according to the present invention.

FIG. 4 is a third partitioning information relationship block diagram of the sensing system and the execution system in the trajectory tracking system according to the present invention.

FIG. 5 is a block diagram of a fourth information partitioning method for a sensing system and an execution system in a trajectory tracking system according to the present invention.

Fig. 6 is a schematic diagram of determining the position of the reference point in the third embodiment of the present invention.

Fig. 7 is a schematic diagram of another method for determining the position of the reference point according to the third embodiment of the present invention.

Fig. 8 is a schematic view of the pitch angle deviation δ x of the reference point in the third embodiment of the present invention.

Fig. 9 is a schematic view of the angular deviation δ y of the advancing direction of the reference point in the third embodiment of the present invention.

Fig. 10 is a diagram of a comprehensive information relationship of a comprehensive driving information driving execution system in which driving information, predetermined task trajectory information, and intervention information are combined according to a third embodiment of the present invention.

Fig. 11 is a schematic diagram of deviation of the control reference point in the fourth embodiment of the present invention.

Fig. 12 is a schematic representation of the original trajectory information in the fifth embodiment of the present invention.

Fig. 13 is a schematic diagram illustrating a general representation of track information according to a fifth embodiment of the present invention.

Fig. 14 is a first schematic diagram of an insufficient capacity of the sensing system.

FIG. 15 is a second schematic diagram of an insufficient capacity of a sensing system.

FIG. 16 is a schematic view of a sensor not on the same joint as the implement.

Fig. 17 is a first schematic view of an under-degree-of-freedom actuator.

Fig. 18 is a second schematic view of an under-degree-of-freedom actuator.

Fig. 19 is a third schematic view of an under-degree-of-freedom actuator.

Detailed Description

The technical solution of the present invention will be further described in detail with reference to the accompanying drawings and preferred embodiments.

Example one

In the method for controlling trajectory tracking according to this embodiment, the tool 2 is attached to the actuator, the sensor 3 is attached to the joint of the actuator or the sensor 3 is directly attached to the tool 2, and the relative position between the sensor 3 and the tool 2 has a certain geometric relationship, which specifically includes the following steps:

the first step is as follows: detecting a track: the original track information of the detection point on the track on the workpiece 1 is acquired by the sensor 3, the sensor 3 involved here can convert the detected visual information into the original track information, and at present, such sensors are generally visual sensors, and the present invention is described by taking a typical structured light visual sensor as an example.

The second step: extracting track information: and extracting the position information of the detection points from the original track information, and expressing the position information by a sensor coordinate system.

For convenience of description, the following description is given by taking an example that a robot tracks a V-groove weld and a sensor 3 is arranged at the front end of a welding gun, where an actuator is the robot, a tool 2 is the welding gun, a trajectory is a weld 4, and a trajectory characteristic line is a weld characteristic line, and the specific structure is shown in fig. 1. The application of the trajectory tracking control method and the corresponding trajectory tracking system is not limited to the welding field, and the method and the system can be applied to tasks of guiding the tool 2 to move along a continuous geometric trajectory, such as tasks of cutting materials, gluing, polishing corners and the like based on specific geometric margins. As shown in fig. 1, the weld cross-sectional information detected by the sensor 3 is concentrated in a plane M-sense, which is defined as a detection section, and a coordinate system O-XYZ is defined as a sensor coordinate system. There is a point on the cross-section of the weld 4 that can represent the weld position, such as the intersection of the two sides on the fillet weld cross-section, the intersection of the lowest V in a V-groove weld, and the like. We will refer to this point representing the position of the weld on the inspection section, denoted as point Q in fig. 1, as the inspection point, and during welding the sensor 3 sweeps over the weld 4, obtaining a series of inspection points：Q ₁ 、Q ₂ 、Q ₃ ……Q _i . The point representing the position of the torch is defined as the torch feature point P, which is generally located on the axis of the torch, outside the tip, at a distance from the tip equal to the electrode extension plus the arc length. The robot moves the torch under the guidance of the series of detection point information of the sensor 3, and welds the torch feature point P along the actual weld 4, where the position and orientation matrix of the torch feature point P is denoted by Tt. Any welding seam can be abstracted into a straight line section or a curve section, namely a track to be tracked by a welding gun characteristic point P in welding, and the track is called as a welding seam characteristic line.

As shown in fig. 1, the coordinate system of the welding gun at the current position is determined as follows: the advancing direction of the welding gun in the welding process is defined as the positive direction of an axis Y-tool', the other two coordinate axes can be defined according to the actual machining process as required, and for convenience of description, the other two coordinate axes are defined in a conventional mode: the X-tool ' axis is vertical to a plane formed by the Y-tool ' axis and the axis of the welding gun, the Z-tool ' axis is determined according to the X-tool ' axis and the Y-tool ' axis, the positive direction of the Z-tool ' axis is the direction away from the welding gun and pointing to the workpiece 1, and finally the positive direction of the X-tool ' axis is determined according to the right-hand rule. In the positive direction of the Y-tool 'axis, the sensor 3 is in front of the welding gun, the coordinate system of the welding gun is the coordinate system P-X-tool' Y-tool 'Z-tool' where the tool 2 is located at the current moment, and the position and posture transformation relation matrix T-sensor between the coordinate system of the sensor 3 and the tool coordinate system is known, so that details of the specific position and posture transformation relation matrix T-sensor are omitted here. If the sensor 3 is attached to other joints of the actuator, the subsequent calculations are still not affected as long as this transformation relation T-sensor is known.

The third step: marking track information: converting the position information expressed by the sensor coordinate system into position information expressed by a coordinate system attached to the track, marking the position information in the coordinate system attached to the track, and storing the position information in an information storage area to form a track position information set { Q }; when the position of the workpiece is not fixed, the coordinate system attached to the track is a workpiece coordinate system, and when the position of the workpiece is fixed, the coordinate system attached to the track is one of a workpiece coordinate system and a world coordinate system.

The robot works in a world coordinate system, and the position and the posture of the welding gun feature point P in the world coordinate system can be represented by a position and posture matrix Tt, which is a comprehensive description of the welding gun 2. In addition to being expressed in the form of a matrix, it is obvious that the position and orientation can also be expressed in other forms, for example, in the form of euler angles, or quaternions. If the workpiece 1 is not attached to the world coordinate system but to other coordinate systems, the trajectory should also be expressed in the corresponding workpiece coordinate system, which is easy to be realized by robotics knowledge and will not be described again.

The conversion process of converting the position information expressed in the sensor coordinate system into the position information expressed in the coordinate system to which the trajectory is attached is:

firstly, converting the position information expressed by a sensor coordinate system into the position information expressed by a tool coordinate system by adopting the following formula: Q-T = (T-sensor) X (Q-s), wherein a vector Q-T represents the description of the detection point Q in a tool coordinate system, Q-s represents the description of the detection point Q in a sensor coordinate system, and T-sensor is a position posture transformation relation matrix between the sensor coordinate system and the tool coordinate system;

secondly, converting the position information expressed by the tool coordinate system into the position information expressed by the coordinate system to which the track is attached, and adopting the following formula: q-w = (Tt) X Q-T = (Tt) X (T-sensor) X (Q-s), wherein a vector Q-w represents the description of a detection point Q point in a world coordinate system or a workpiece coordinate system, and Tt represents a position posture matrix of a welding gun characteristic point P point.

Obviously, in the above four formulas, the coordinates of the point should be homogeneous coordinates, not three-dimensional coordinates, and other symbols are not used for expression only for brevity. With the advance of the welding gun 2 with the sensor 3, a welding seam 4 completely described by a series of Q [ i ] point positions is obtained, a series of lines sequentially connected by the Q [ i ] points can represent the actually detected characteristic line of the welding seam, the description information of the current Q point is added into a track position information storage area to form a set, and the set is marked as { Q }, and is used for later steps.

The fourth step: determining a target: and determining the target position and the attitude of the tool at the next moment according to the track position information set { Q }, which is as follows: setting the target position of the tool 2 at the current moment as a point P, wherein the tool coordinate system of the point P is P-X-tool ' Y-tool ' Z-tool ', a plane perpendicular to the track characteristic line is made through the point P, the intersection point of the plane and the track characteristic line is a reference point Ref-A, the reference point Ref-A is taken as a tracking target, and the position of the reference point Ref-A is taken as the target position of the tool at the next moment.

The fifth step: driving a tool: and the actuator moves the target position of the tool 2 from the target position at the current moment to a Ref-A point according to the position information of the reference point Ref-A, and forwards moves by a step delta = V x t along the positive direction of the Y-tool' axis by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool.

And a sixth step: and circulating the steps until the tracking of the whole track is completed.

The fourth step determines that the target position and the posture of the tool 2 at the next moment can be specifically expanded according to the trajectory position information set { Q }, where the first specific expansion is as follows: setting a target position of the tool 2 at the current moment as a point P, setting a tool coordinate system of the point P as P-X-tool ' Y-tool ' Z-tool ', making a plane perpendicular to the track characteristic line through the point P, taking an intersection point of the plane and the track characteristic line as a reference point Ref-A, and determining the tool coordinate system P-X-tool Y-tool Z-tool at the reference point Ref-A by taking the reference point Ref-A as a tracking target: making a tangent line of the track characteristic line by passing through a reference point Ref-A, and taking the tangent line of the track characteristic line and a plane passing through a straight line parallel to the axis of the Z-tool' as a track segmentation plane; a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersecting line of the plane and the track segmenting surface is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; and taking the position and the posture of the reference point Ref-A as the target position and the posture of the tool at the next moment. At this time, in the fifth step, the actuator moves the target position of the tool 2 from the target position at the current time to the point Ref-a based on the position and posture information of the reference point Ref-a, and moves forward by one step Δ = V × t in the positive direction of the Y-tool axis starting from Ref-a.

In the first specific development, because the detection link has no attitude information N, the track segmentation surface cannot exist, and the attitude cannot be adjusted, and in the first specific development, the missing N is compensated by using the attitude information of the current tool Z-tool' axis, so that the track segmentation surface can adjust the attitude.

Ideally, the position and posture relationship between the axis of the welding gun and the weld 4 is determined by the welding process, and then the axis of the welding gun is determined when the weld 4 is welded. Considering a small weld segment passing through the detection point Q, the small weld segment can be approximately replaced by a small straight line passing through the point Q, so that the following definition is given: and a plane M-seam passing through the welding gun axis in the ideal state of the point Q of the detection point and a straight line representing the welding seam is a welding seam dividing plane. Taking welding as an example, the trajectory split surface mentioned in the first specific development is the weld split surface.

Therefore, besides obtaining the trajectory segmentation surface by using the method of using the gesture information of the current tool Z-tool 'axis to compensate the missing N in the first unfolding method, a vector with the same direction as the Z-tool' axis can be used as the gesture information of the detection point in the second step, and the gesture information can be expressed by using a sensor coordinate system. And then converting the attitude information expressed by the sensor coordinate system into attitude information expressed by a coordinate system attached to the track through a third step, marking the attitude information in the coordinate system attached to the track, and storing the attitude information in an information storage area to form a track attitude information set { N }, wherein the track attitude information set and the track position information set jointly form a track position attitude information set { Q, N }.

At this time, in the fourth step, according to the trajectory position and posture information set { Q, N }, a tool coordinate system P-X-toolY-toolZ-tool located at the reference point Ref-a is determined: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool 2; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; and taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment. At this time, the fifth step actuator moves the target position of the tool 2 from the target position at the current time to the point Ref-a according to the position and posture information of the reference point Ref-a, and moves forward by one step Δ = V × t along the positive direction of the Y-tool axis with Ref-a as the starting point.

For the sake of convenience, the new information is generated by the sensor 3 at the same time as the control of the actuator, and the new information is likely to be generated by the sensor 3 at a slower time than the control of the actuator in actual use. Since the latest detected point information is not required for determining the information of the corrected trajectory, it is only necessary to determine the target at the next time directly from the set { Q } of trajectory position information when no new monitoring information is generated.

The analysis of the above steps is performed by taking the axis of the welding gun perpendicular to the trajectory as an example, and is only for the sake of brevity. If the actual welding process requires working at a certain angle other than 90 degrees, the tool is easily adjusted to the angle through matrix transformation, so that the method is suitable for the trajectory tracking control method.

In the above description, there is no period of initial seek to the start of a trajectory involving tracking. At this time, no track information is accumulated, the tool 2 needs to be guided to the vicinity of the starting point of the actual track in advance through other measures, then the start point of the search track advancing along the Y-tool direction is initiated, and the tracking process is really started after track information of at least two detection points is accumulated (a small piece of track information is obtained) to ensure that the sensor can sense the starting point of the track in the advancing process.

The above method is described only by way of example of a V-groove weld, but is not limited thereto. For fillet weld, lap weld, butt weld, etc., it is applicable as long as the weld characteristic point P on the detection section is defined. For tasks such as gluing, the above trajectory tracking control method can be fully referred to.

The above description is made on the premise that the position of the workpiece 1 is fixed, when the position of the workpiece 1 is not fixed, the coordinate system of the sensor 3 and the coordinate system of the tool 2 can be converted into a form of reference by the workpiece coordinate system, which is equivalent to the form of fixing the position of the workpiece, so that the trajectory tracking control method described in the present application can be used for both fixing the position of the workpiece and not fixing the position of the workpiece.

The track tracking system for realizing the track tracking control method comprises a sensor 3, a tool 2 and an actuator, wherein the tool 2 is attached to the actuator, the actuator drives the tool 2 to move, and finally the target position of the tool 2 tracks a track. The sensor 3 is attached to the joint of the actuator or the sensor 3 is directly attached to the tool 2, the relative position between the sensor 3 and the tool 2 has a determined geometric relationship, the system further comprises an information detection processor 1011, an information application processor 1012, a forward kinematics module 2011 and an inverse kinematics module 2012, and the information application processor 1012 further comprises an information storage area;

the sensor 3 collects the original track information of the track on the workpiece 1;

the information detection processor 1011 extracts the position information of the track according to the original track information collected by the sensor 3 and expresses the position information by a sensor coordinate system;

the positive kinematics module 2011 obtains the joint angle information of the actuator when the target position of the tool 2 is at the current target position, and calculates the target position and the posture information of the tool 2 at the current time according to the joint angle information of the actuator;

the information application processor 1012 converts the position information of the track expressed by the sensor coordinate system into position information expressed by the coordinate system to which the track is attached, then marks the position information in the coordinate system to which the track is attached, and stores the position information in the information storage area to form a track position information set { Q };

the information application processor 1012 calls the target position and attitude information of the tool 2 in the positive kinematics module at the current moment, and determines the target position and attitude information of the tool 2 at the next moment according to the target position and attitude information of the tool 2 at the current moment and the track position information set { Q };

the inverse kinematics module 2012 calls the target position and posture information of the tool 2 at the next time in the information application processor, and calculates the joint angle information of the actuator when the target position of the tool 2 is at the target position at the next time according to the target position and posture information of the tool 2 at the next time.

The track tracking system can be further divided into a sensing system 5 and an execution system 6, the functions of the sensing system 5 and the execution system 6 are definitely separated, and the two systems can realize track tracking only by frequently exchanging information. For convenience of description, the sending of information from the sensing system 5 to the executing system 6 is defined as "downloading", the sending of information from the executing system 6 to the sensing system 5 is defined as "uploading", and the specific division of the sensing system 5 and the executing system 6 in the trajectory tracking system is as follows:

as shown in fig. 2, the first division mode is the division of the operation mode of providing information by the sensing system: the sensor 3 and the information detection processor 1011 form a sensing system 5, and a first external communicator and a first information interface end are arranged in the sensing system 5; the information application processor 1012, the forward kinematics module 2011, the inverse kinematics module 2012 and the actuator form an execution system 6, a second external-to-external communicator and a second information interface terminal are arranged in the execution system 6, and the sensing system 5 and the execution system 6 are electrically connected through the first information interface terminal and the second information interface terminal.

During operation, the sensing system 5 does not need to upload information, i.e. the execution system 5 does not need to upload any information to the sensing system 5, but only the sensing system 5 needs to download the position information of the track expressed by the sensor coordinate system to the execution system 6. The sensing system 5 mainly undertakes track position information acquisition and extraction tasks, and the execution system 6 undertakes subsequent information processing work. When the tool 2 is in work, the sensing system 5 transmits track position information expressed by a sensor coordinate system to the execution system 6 through the first external communicator and the first information interface end, the execution system 6 receives and processes the track position information expressed by the sensor coordinate system and transmitted by the sensing system 5 through the second information interface end and the second external communicator, the target position and posture information of the tool at the next moment are obtained after processing, and the actuator drives the tool 2 according to the information to enable the target position of the tool 2 to move from the current target position to the next target position.

The dividing mode of the track tracking system has the advantages that: the sensing system 5 has a simple structure, and the volume of the sensing system can be further reduced.

As shown in fig. 3, the second division is a division of the driving mode of the sensing system direct drive execution system: the sensor 3, the information detection processor 1011 and the information application processor 1012 form a sensing system 5, and a first external communicator and a first information interface end are arranged in the sensing system 5; the positive kinematics module 2011, the inverse kinematics module 2012 and the actuator form an execution system 6, a second external communicator and a second information interface end are arranged in the execution system 6, and the sensing system 5 and the execution system 6 are electrically connected through the first information interface end and the second information interface end.

When the tool is in operation, the execution system 6 transmits the current-time position and posture information of the tool to the sensing system 5 through the second external communicator and the second information interface end, the sensing system 5 receives and processes the current-time target position and posture information of the tool transmitted by the execution system 6 through the first information interface end and the first external communicator to obtain the next-time target position and posture information of the tool, then the sensing system 5 feeds the next-time target position and posture information of the tool back to the execution system 6, and the actuator drives the tool 2 to enable the target position of the tool 2 to move from the current-time target position to the next-time target position.

As shown in FIG. 4, the third division is divided by the sensing system and the driving mode of the implementation system contract model: the sensor 3, the information detection processor 1011, the information application processor 1012 and the positive kinematics module 2011 form a sensing system 5, and a first external communicator and a first information interface end are arranged in the sensing system 5; the inverse kinematics module 2012 and the actuator form an execution system 6, a second external communicator and a second information interface end are arranged in the execution system 6, and the sensing system 5 and the execution system 6 are electrically connected through the first information interface end and the second information interface end.

During operation, the execution system 6 transmits the joint angle information of the actuator at the current target position of the tool 2 to the sensing system 5 through the second external communicator and the second information interface, the sensing system 5 receives the joint angle information of the actuator at the current target position of the tool 2 uploaded by the execution system 6 through the first external communicator and the first information interface, and since the uploaded information is not enough to directly solve the target position and the posture of the tool 2 at the next time, a kinematic model of the actuator needs to be appointed in advance, and parameters which are coordinated with each other are set for the sensing system 5 and the execution system 6. And the sensing system 5 solves the target position and attitude information of the tool 2 at the next moment according to the uploaded joint angle information of the actuator and the kinematic model of the actuator, and feeds back the target position and attitude information to the execution system 6, and the actuator drives the tool 2 to enable the target position of the tool 2 to move from the target position at the current moment to the target position at the next moment.

As shown in fig. 5, the fourth division is divided by the sensing system and the actuating system contract model and the sensing system is divided by the driving mode of the joint angle control actuating system: the sensor 3, the information detection processor 1011, the information application processor 1012, the forward kinematics module 2011 and the inverse kinematics module 2012 form a sensing system 5, and a first external communicator and a first information interface end are arranged in the sensing system 5; the actuator forms an execution system 6, a second external communicator and a second information interface end are arranged in the execution system 6, and the sensing system 5 and the execution system 6 are electrically connected through the first information interface end and the second information interface end.

During operation, the executing system 6 transmits the joint angle information of the actuator of the target position of the tool 2 at the current target position to the sensing system 5 through the second external communicator and the second information interface, the sensing system 5 receives the joint angle information of the actuator of the target position of the tool 2 at the current target position uploaded by the executing system 6 through the first external communicator and the first information interface, and since the uploaded information is not enough to directly solve the joint angle information of the actuator of the tool 2 at the next target position, a kinematic model of the actuator needs to be agreed in advance, and parameters which are coordinated with each other are set for the sensing system 5 and the executing system 6. And the sensing system 5 solves the joint angle information of the actuator when the tool is positioned at the target position at the next moment according to the uploaded joint angle information of the actuator and the kinematic model of the actuator, and feeds the joint angle information back to the execution system 6, and the actuator drives the tool 2 to enable the target position of the tool 2 to move from the target position at the current moment to the target position at the next moment.

When the second division mode, the third division mode and the fourth division mode work, the kinematic state information uploaded by the execution system at the same moment needs to be received for standby. The closer the acquisition time of the kinematic state information is to the acquisition time of the sensor, the better, and the advantage of the kinematic state information is that the uploaded information can be more accurately utilized.

The first information interface end and the second information interface end related in the four dividing modes can be made into a convex end and a concave end of the information interface, and when the first information interface end is the convex end, the second information interface end is the concave end; when the first information interface end is a concave end, the second information interface end is a convex end. In the actual use process, the whole body after the first information interface end and the second information interface end are electrically connected can be called an information interface.

Example two

The difference between the present embodiment and the first embodiment is: in this embodiment, on the basis of the reference point Ref-a, the current control reference point Ref-a is not used as the tracking target, but the control reference point Ref-a-next after the control interval time t is used as the tracking target. The specific determination method for determining the target position of the tool 2 at the next time according to the trajectory position information set { Q } in this embodiment is as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by passing through a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as an axis Y-tool, wherein the positive direction of the axis Y-tool points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; and taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment.

In the fifth step, the actuator moves the target position of the tool 2 from the target position at the current moment to the target position at the next moment according to the position and posture information of the control reference point Ref-A-next.

Other steps of the trajectory tracking control method, specific development of each step, and a trajectory tracking system for implementing the trajectory tracking control method are the same as those in the first embodiment, and are not described again.

Instead of using the current control reference point Ref-a as the tracking target, the control reference point Ref-a-next after the control interval time t is used as the tracking target, which has the advantage of better tracking performance by using the predicted deviation that will occur at the next control adjustment time. In theory, all the positions and postures reached by the controlled guidance are ideal, and no deviation is generated, namely, the P [ i +1] point coincides with the Ref-A-next point, and the deviation actually refers to the difference between the current time state and the next time state. The actual errors are only the errors generated in the track information detection and the servo control errors generated when the system control is not executed.

EXAMPLE III

The difference between the present embodiment and the first embodiment is: in the fourth step of determining the target, the target position and posture of the tool 2 at the next time are determined according to the trajectory position information set { Q }, and then the deviation information of the position and posture of the tool 2 between the target position at the current time and the target position of the tool at the next time is further obtained. The specific track tracking control method comprises the following steps:

the first step is as follows: detecting a track: the original track information of the track detection point on the workpiece is acquired by the sensor 3.

The second step is that: extracting track information: and extracting the position information of the detection point from the original track information, and expressing the position information by a sensor coordinate system.

The fourth step: determining a target: the target position and attitude of the tool 2 at the next time are determined from the set { Q } of trajectory position information, and position and attitude deviation information between the target position of the tool 2 at the current time and the target position of the tool 2 at the next time is obtained, the position and attitude deviation information being a lateral deviation Deltax or a lateral deviation Deltax in combination with at least one of a forward deviation Deltay, a height deviation Deltaz, a pitch angle deviation Deltax, and a forward direction angle deviation Deltaz.

The specific way of determining the target position and posture of the tool 2 at the next time point according to the trajectory position information set { Q } is as follows: setting a target position of a tool at the current moment as a point P, setting a tool coordinate system of the point P as P-X-tool ' Y-tool ' Z-tool ', making a plane perpendicular to a track characteristic line through the point P, taking an intersection point of the plane and the track characteristic line as a reference point Ref-A, taking the reference point Ref-A as a tracking target, and determining the tool coordinate system P-X-tool Y-tool Z-tool of the tool at the reference point Ref-A: making a tangent line of the track characteristic line by passing through a reference point Ref-A, and taking the tangent line of the track characteristic line and a plane passing through a straight line parallel to the axis of the Z-tool' as a track segmentation plane; a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; and taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment.

The specific solving process of the position and attitude deviation information is as follows: setting a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' of a target position P point of the tool at the current moment,

Δ z is: the projection length of the vector from the point P to the reference point Ref-A in the Z-tool' axis direction.

Besides the position deviation, the welding gun may have a posture angle deviation, and several parameters for realizing the robot track control are described by using the tool coordinate system as a reference system. As shown in fig. 6, a plan view passing through the X-tool 'axis and the Y-tool' axis, wherein a line segment S-xy is a projection of a weld characteristic line on the plane. Since the small size range near the welding gun characteristic point is considered, the welding seam characteristic line can be approximate to a straight line segment, and S-xy can also be regarded as the straight line segment. The angular deviation of the torch adjustment about the Z-tool 'axis, which is the angle of S-xy with the Y-tool' axis, is then required to be deltaz. Determining other deviations also requires selecting a control reference point on the weld signature line, such point being the position Ref-a in fig. 6. Since the angle δ z is small in actual operation, the control reference point may also be selected as the position Ref-B in fig. 7, which is a reference point on the plane passing through P and the X-tool axis, and this will not bring a large error to the deviation calculation. Similarly, when other deviation amounts are calculated, because the included angle between the weld characteristic line and each coordinate axis is small, the control reference point can always select the Ref-A point or the Ref-B point, and large errors cannot be caused. Namely: δ z is: and the included angle between the projection S-xy of the tangent line positioned at the reference point Ref-A or the reference point Ref-B on the track characteristic line in the plane passing through the X-tool 'axis and the Y-tool' axis direction.

FIG. 8 shows a cross-sectional view through the Y-tool 'axis and the Z-tool' axis, wherein the line segment S-yz is the projection of the weld seam feature line on the cross-sectional view. The angular deviation of the torch adjustment about the X-tool 'axis is then required to be δ X, i.e. the angle between S-yz and the Y-tool' axis. That is, δ x is: and the included angle between the projection S-yz of a tangent line positioned at the reference point Ref-A or the reference point Ref-B on the track characteristic line in the plane passing through the Y-tool ' axis and the Z-tool ' axis and the Y-tool ' axis direction.

As shown in FIG. 9, when the section S-xz is the projection of the weld feature line on the section plane passing through the X-tool 'axis and the Z-tool' axis, the angular deviation of the welding torch required to be adjusted around the Y-tool 'axis is δ Y, i.e., the included angle between S-xz and the Z-tool' axis. Fig. 9 also shows the position adjustment amount Δ x and Δ z of the welding torch with Ref-B as a reference point.

The deviation can be expressed synthetically as a six-dimensional vector, which is noted as:

d-tool = [. Δ x,. Δ y,. Δ z, δ x, δ y, δ z ], where Δ y =0.

This is a complete description of the tool trajectory deviation and is a representation in the tool coordinate system. Since it can be used directly to drive the actuator, it can also be referred to as drive information.

Since Δ Y =0, it is obvious that the driving information in the Y-tool coordinate axis direction is supplemented inside the execution system to advance the tracking, and the robot moves along the Y-tool coordinate axis direction at the welding speed V when working. In this case, it can be considered that the tracking is a comprehensive result of executing the external drive command D-tool and the internal propulsion drive command.

The speed information V of the track tracked by the tool is received from the execution system at the current moment, or is determined by the sensing system, or is set by the execution system, or is appointed between the execution system and the sensing system, and the core idea of the invention is not influenced. In a special case, the execution system ignores the speed adjustment information Δ y itself, and when V =0, the execution system can also bring about the special required effects: and if the external correction quantity of the movement speed of the robot is changed into delta Y = V x t in the Y-tool' axis direction, wherein t is the adjustment interval time of each correction control, the robot can realize the tracking only by external D-tool driving. The robot is a device driven by an external sensing system completely. If another six-dimensional vector D-tool-2 with the component meaning equivalent to that of the D-tool is generated by other means, the method can also be used for driving the execution system, and as shown in FIG. 10, the execution system can even be driven by combination information of various driving information.

The fifth step: driving a tool: and the actuator moves the target position of the tool 2 from the target position at the current moment to a Ref-A point according to the position and attitude deviation information of the reference point Ref-A, and moves forwards by a step delta = V t along a preset direction by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool. The modified driving information for moving the target position of the tool 2 from the target position at the current moment to the Ref-a point is driving information D-tool, and the driving information D-tool at least contains one effective component: a vector of Δ x, and the driving information D-tool is a vector containing at most five significant components: vectors for Δ x, Δ y, Δ z, δ x, δ z. Wherein when the drive information D-tool does not include δ x or δ z, the predetermined direction is a direction which is naturally determined due to the displacement adjustment, which means here that the adjustment of the attitude is disregarded, and the position adjustment is made only by at least one of Δ x, Δ y, Δ z, whereby the position change determines the advancing direction of the tool; when the driving information D-tool contains deltax or deltaz, the predetermined direction is a positive direction of the Y-tool axis.

In the trajectory tracking scheme for realizing the fully automatic control, the term "fully automatic control" means that the tracking can be automatically adjusted according to the trajectory characteristics without depending on a pre-written trajectory description program (for example, a teaching program). The "teaching trace" information in fig. 10 does not work at this time. Namely: the correction drive information may be only the individual drive information D-tool.

In an environment where no interference collision during tracking cannot be confirmed, those foreseeable obstacles can be avoided by simple teaching information, and in this case, the "teaching trajectory" information in fig. 10 is used, and the posture of the tool or some degrees of freedom in the posture, for example, δ y or δ y and δ z, are determined from the information, so that no collision can be ensured. Even in this case, the teaching information is very simplified, not the conventional complicated teaching information for determining the travel locus. That is, the correction drive information may be integrated drive information in which the drive information D-tool is combined with at least one of predetermined task trajectory information and intervention information.

Therefore, the modified driving information in the fifth step may be only the driving information D-tool, or may be integrated driving information in which the driving information D-tool is combined with at least one of the predetermined task trajectory information and the intervention information.

If the executing system receives track correction information which is formed by manual operation and defined by the tool coordinate system of the actuator as a reference system, the executing system can also receive track correction information which is formed by manual operation and defined by the tool coordinate system of the actuator as the reference system, the track correction information is combined with the received deviation control information provided by the external sensing system, and then track correction is carried out by the combined information; the method of combining the deviation information is determined according to different tasks. In the figure, the symbol Σ does not mean that the respective control vectors are simply added, but means that information is fused, and there may be a change in the ratio of the control amount, which can be increased or decreased, or the like. For example, a human employs a six-dimensional force/torque sensor that generates up to 6-dimensional information relative to the operator: the hand pushing force/moment sensor sends out positive delta y information, and sends out negative delta y information by pulling, so that the magnitude of delta y is in direct proportion to the pushing and pulling force; the information is added with the corresponding component of the D-tool or weighted addition, so that the human intervention on the tracking speed can be realized; similarly, the adjustment of temporarily deviating the tool from the tracking track can be realized by using a lateral push-pull force/torque sensor; the torque generated by the torque force/moment sensor can then be used to adjust the attitude of the tool. It is of course also possible to "filter" out undesired control information and thus suppress undesired changes in the information. To avoid unwanted interference in one dimension, it can be masked out, for example, to ignore inadvertently generated yaw adjustment information to the tool. This is the principle of the incorporation of the deviation information. A system similar to a joystick may also be used to elicit such human intervention information.

The trajectory tracking system for implementing the trajectory tracking control method in this embodiment includes a sensor 3, a tool, and an actuator, where the tool 2 is attached to the actuator, the sensor 3 is attached to a joint of the actuator or the sensor 3 is directly attached to the tool 2, and a relative position between the sensor 3 and the tool has a certain geometric relationship, and further includes an information detection processor 1011, an information application processor 1012, a forward kinematics module 2011, and an inverse kinematics module 2012, and the information application processor 1012 further includes an information storage area;

the information detection processor 1011 extracts the position information of the track according to the original track information acquired by the sensor and expresses the position information by a sensor coordinate system;

the positive kinematics module 2011 acquires the joint angle information of the actuator when the target position of the tool 2 is at the target position at the current moment, and calculates the target position and the posture information of the tool 2 at the current moment according to the joint angle information of the actuator;

the information application processor 1012 calls the position and attitude information of the tool at the current moment in the positive kinematics module, determines the target position and attitude information of the tool at the next moment according to the position and attitude information of the tool at the current moment and the track position information set { Q }, and calculates the position and attitude deviation information between the position of the tool at the current moment and the target of the tool at the next moment;

the inverse kinematics module 2012 calls the position and posture deviation information between the target position of the tool 2 at the current time and the target position of the tool 2 at the next time in the information application processor, and calculates the joint angle information when the target position of the tool 2 is at the target position at the next time according to the position and posture deviation information between the target position of the tool 2 at the current time and the target position of the tool 2 at the next time.

The track tracking system can be further divided into a sensing system 5 and an execution system 6, the functions of the sensing system 5 and the execution system 6 are explicitly disclosed, and track tracking can be well realized only by frequently exchanging information between the sensing system 5 and the execution system 6. For description of aspects, sending information from the sensing system 5 to the execution system 6 is defined as "downloading", sending information from the execution system 6 to the sensing system 5 is defined as "uploading", and the specific division of the sensing system 5 and the execution system 6 in the trajectory tracking system is the same as the four division methods in the first embodiment, and the specific working method is also the same, which is not described herein again.

Example four

The present embodiment is different from the third embodiment in that: in this embodiment, on the basis of the reference point Ref-a, the current control reference point Ref-a is not used as the tracking target, but the control reference point Ref-a-next after the control interval time t is used as the tracking target. The specific way of determining the target position of the tool at the next time according to the trajectory position information set { Q } in this embodiment is as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' of the tool positioned at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by passing through a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment; and calculating the position and attitude deviation information between the position of the tool at the current moment and the target position of the tool at the next moment according to the position and attitude information of the control reference point Ref-A-next, wherein the position and attitude deviation information is that the lateral deviation delta x or the lateral deviation delta x is combined with at least one of the advancing deviation delta y, the height deviation delta z, the pitching angle deviation delta x and the advancing direction angle deviation delta z.

The specific solving process of the position and attitude deviation information is as follows: setting the tool coordinate system of the target position P point at the current moment as P-X-tool ' Y-tool ' Z-tool ',

δ z is: an included angle between the projection S-xy of a tangent line positioned at the control reference point Ref-A-next on the track characteristic line in a plane determined by an X-tool ' axis and a Y-tool ' axis and the direction of the Y-tool ' axis;

At the moment, in the fifth step, the actuator enables the target position of the tool to move from the target position at the current moment to the target position at the next moment according to the position and posture information of the control reference point Ref-A-next.

Other steps of the trajectory tracking control method, specific development of each step, and a trajectory tracking system for implementing the trajectory tracking control method are the same as those in the embodiment, and are not described again.

Instead of using the current control reference point Ref-a as the tracking target, the control reference point Ref-a-next after the control interval time t is used as the tracking target, and the meanings of Δ x, Δ y, and δ z are shown in fig. 11. In this case, the reference points for calculating other control quantities are modified accordingly (only Ref-a-next is used as Ref-a point, and the above calculation is performed), and the curve trajectory is used to indicate that the value of δ z is different from that of Ref-a as the tracking target. In FIG. 11, Δ is the distance between Ref-A and Ref-A-next in three dimensions. This has the advantage that the tracking performance is better, using the predicted deviation that will occur at the next control adjustment instant for control: in theory, all the positions and postures reached by the controlled guidance are ideal, and no deviation is generated, namely, the P [ i +1] point coincides with the Ref-A-next point, and the deviation actually refers to the difference between the current time state and the next time state. The actual errors are only the errors generated in the track information detection and the servo control errors generated by the fact that the execution system cannot control the position.

The sensor 3 involved in the trajectory tracking method and the trajectory tracking system according to each of the above-described first, second, third, and fourth embodiments has only to have the ability to sense the deviation of the target trajectory in the direction away from the tool and the deviation in the directions on both sides of the trajectory, and even if the sensor cannot sense the characteristic that the workpiece on which the trajectory is located is "in which direction" at the detection point on the surface, it is referred to herein as "attitude" of the trajectory, trajectory tracking can be performed well.

The sensors involved in the trajectory tracking methods and the corresponding trajectory tracking systems in the fifth, sixth, seventh and eighth embodiments described below need to have the ability to sense both the deviation of the target trajectory in the direction away from the tool and the deviation in the directions on both sides of the trajectory, and also the "direction" of the trajectory at the detection point of the workpiece surface, which is referred to herein as the "attitude" of the trajectory.

EXAMPLE five

The difference between the present embodiment and the first embodiment is: the sensor in this embodiment is capable of sensing not only the deviation of the target trajectory in the direction away from the tool, the deviation in the directions on both sides of the trajectory, which are collectively referred to as the positional information of the trajectory, but also the characteristic of "which direction" the surface of the workpiece on which the trajectory is located at the detection point, that is, the attitude information. The specific development of the steps of the trajectory tracking control method is the same as that of the first embodiment, and is not described herein again.

The specific trajectory tracking control method described in this embodiment includes the following steps:

the first step is as follows: detecting a track: the original track information of the detection point on the track on the workpiece 1 is acquired by the sensor 3.

The second step is that: extracting track information: and extracting the position information of the detection point and the attitude information of the track segmentation surface at the detection point from the original track information, and expressing the position information and the attitude information by a sensor coordinate system.

During welding, the sensor 3 sweeps over the weld joint 4 to obtain a series of detection points Q, the robot moves with the welding gun under the guidance of the series of detection point information of the sensor to weld the characteristic point P of the welding gun along the actual weld joint, and the position and posture matrix of the characteristic point P of the welding gun is represented by Tt.

Any weld can be abstracted into a straight line segment or a curve segment, namely a track to be tracked in welding, which is called a weld characteristic line. The weld tracking is to make the characteristic point P not depart from the characteristic line of the weld seam and to make the axis of the welding gun and the characteristic line of the weld seam maintain the specific angle relation required by the welding process during the movement of the welding gun.

Ideally, the position and posture relationship between the axis of the welding gun and the welding seam is determined by the welding process, and then the axis of the welding gun is determined when the welding seam is welded. Considering a small weld segment passing through the detection point Q, the small weld segment can be approximately replaced by a small straight line passing through the point Q, and then: and a plane M-seam passing through the welding gun axis in the ideal state of the point Q of the detection point and a straight line representing the welding seam is a welding seam dividing plane. The cross section outline of the welding seam in the detection section M-sense is detected by the sensor, the intersecting line of the welding seam dividing plane M-seam and the detection section M-sense can be calculated according to the outline, the intersecting line is expressed as a vector N-seam, and the vector N-seam can represent the direction of the welding seam dividing plane and is called as a welding seam attitude vector or welding seam attitude for short.

In fact, unless the detection section M-sense is perpendicular to the weld splitting plane M-seam, a plurality of weld attitude vectors N-seam can be obtained through the Q point. But this does not affect the mission of N-team: the method is used for determining the weld split plane M-seam determined by the Q point through the N-seam and a small-segment track, so that the obtained N-seam can play the same role as long as the M-sense intersects with the M-seam.

FIG. 1 shows a special state in which the coordinate axes of the sensor coordinate system are parallel to the weld seam and are free of deviations, while FIG. 12 shows a general state in which the measured values are shown on the detection section M-sense

The weld seam contour is described by a series of points or line segments, and the information is rich. We want to extract the most efficient and minimal information to represent the characteristics of the weld. We select the position information of the Q point and the vector N-seam to jointly represent the characteristics of the weld. It can be seen that if the description of the Q point in the sensor coordinate system is represented by the vector Q-s, then Q-s = [ Xs,0, zs ].

Rotating the vector from point a to point e by 90 counterclockwise, depending on the geometric relationship, can be used to express the vector N-seam, i.e., -dz/L,0, dx/L. And representing the description of the N-seam in the sensor coordinate system by using a vector N-s, then N-s = [ -dz/L,0, dx/L ].

It can be seen that when it is necessary to send the weld information Q-s, N-s described in the sensor coordinate system to the execution system, only 4 data can be transmitted, since 2 0's in their coordinate representation can be obtained by default. Further, N-s can also be normalized to a unit vector, and then 1 transmission data can also be reduced by normalization. Of course, the pose information N-s can also be expressed in terms of 1 angle δ y.

The third step: marking track information: converting the position and posture information expressed by the sensor coordinate system into position and posture information expressed by a coordinate system attached to the track, marking the position and posture information in the coordinate system attached to the track, and storing the position and posture information in an information storage area to form a track position and posture information set { Q, N }; when the position of the workpiece is not fixed, the coordinate system attached to the track is a workpiece coordinate system, and when the position of the workpiece is fixed, the coordinate system attached to the track is one of a workpiece coordinate system or a world coordinate system.

The robot works in a world coordinate system, and in the world coordinate system, the position and the posture of the welding gun characteristic point P can be represented by a position posture matrix Tt, so that the position posture matrix Tt is a comprehensive description of the welding gun. Besides being expressed in the form of a matrix, it is obvious that the position and orientation can also be expressed in other forms, for example, in the form of euler angles, or quaternions, etc. The conversion process of converting the position and orientation information expressed by the sensor coordinate system into the position and orientation information expressed by the coordinate system to which the trajectory is attached is as follows:

firstly, converting the position and posture information expressed by a sensor coordinate system into the position and posture information expressed by a tool coordinate system by adopting the following formula:

Q-t =（T-sensor）X（Q-s）

N-t =（T-sensor）X（N-s）

the vector Q-T represents the description of the target position at the next moment in a tool coordinate system, the vector Q-s represents the description of the target position at the next moment in a sensor coordinate system, and the T-sensor is a position posture transformation relation matrix between the sensor coordinate system and the tool coordinate system; the vector N-s represents the description of the vector N-team in the sensor coordinate system, and the vector N-t represents the description of the vector N-team in the tool coordinate system;

secondly, converting the position and posture information expressed by the tool coordinate system into the position and posture information expressed by the coordinate system attached to the track, and adopting the following formula:

Q-w =（Tt）X Q-t =（Tt）X（T-sensor）X（Q-s）

N-w =（Tt）X N-t =（Tt）X（T-sensor）X（N-s）

wherein the vector Q-w represents the description of the target position at the next moment in the world coordinate system or the workpiece coordinate system, tt represents the position and posture matrix of the tool at the current moment position P point, and the vector N-w represents the description of the vector N-seam in the world coordinate system or the workpiece coordinate system.

Obviously, in the above four formulas, the coordinates of the point should be homogeneous coordinates, not three-dimensional coordinates, and other symbols are not used for expression only for brevity. Therefore, as shown in FIG. 13, as the welding gun advances with the sensor, a weld completely described by a series of Q [ i ] point positions and their attached vectors N-seam (abbreviated as "Ni") is obtained, and a series of lines connecting the Q points in sequence can represent actually detected weld characteristic lines; the curved surface formed by each N-seam can represent the actually detected weld split surface. And adding the description information of the current Q point into a track position and posture information storage area to form a set, marking as { Q, N }, and reserving for subsequent steps.

If the workpiece is not attached to the world coordinate system but to other coordinate systems, the trajectory should also be expressed in the corresponding workpiece coordinate system, which is easy to be realized by robotics knowledge and is not described again.

The case of welding on a twisted, non-developable surface as shown in fig. 13 may represent a method of processing an arbitrary space curve trajectory. The orientation of the series of vectors N-team is inverted in the figure so as not to obscure the weld signature lines. The direction in which the welding gun advances during welding is defined as the positive direction of the Y-tool axis of the tool coordinate system. In principle, the other two coordinate axes can be arbitrarily defined, and for the sake of brevity, the following assumptions are made here in a customary manner: the X-tool shaft is vertical to the plane formed by the Y-tool shaft and the axis of the welding gun, the positive direction of the Z-tool is the direction away from the welding gun and pointing to the workpiece, and finally the positive direction of the X-tool is determined according to the right-hand rule.

The fourth step: determining a target: and determining the target position of the tool at the next moment according to the track position and posture information set { Q, N }, which is as follows: setting the target position of the tool at the current moment as a point P, making a plane perpendicular to a track characteristic line by passing the point P, taking the intersection point of the plane and the track characteristic line as a reference point Ref-A, taking the reference point Ref-A as a tracking target, and determining a tool coordinate system P-X-toolY-toolZ-tool positioned at the reference point Ref-A: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the reference point Ref-A as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; the sensor detects the section contour of a track line in a detection section M-sense of the sensor at a Ref-A point, an intersection line of a track division plane M-sea and the detection section M-sense is calculated according to the section contour, the intersection line is represented as a vector N-sea, the vector N-sea is the posture of the tool at a reference point Ref-A, and the position and posture information of the reference point Ref-A is used as the target position and posture of the tool at the next moment.

The fifth step: driving a tool: and the actuator moves the target position of the tool from the target position at the current moment to a Ref-A point according to the position and posture information of the reference point Ref-A, and moves forwards by a step delta = V t along the positive direction of the Y-tool axis by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool.

The trajectory tracking system for implementing the trajectory tracking control method in this embodiment includes a sensor 3, a tool 2, and an actuator, where the tool is attached to the actuator, the sensor 3 is attached to a joint of the actuator or the sensor 3 is directly attached to the tool, and a relative position between the sensor 3 and the tool 2 has a certain geometric relationship, and further includes an information detection processor 1011, an information application processor 1012, a forward kinematics module 2011 and an inverse kinematics module 2012, and the information application processor 1012 further includes an information storage area;

the sensor 3 collects the original track information of the track on the workpiece;

the information detection processor 1011 extracts the position and posture information of the track according to the original track information acquired by the sensor 3 and expresses the position and posture information by a sensor coordinate system;

the positive kinematics module 2011 acquires the joint angle information of the actuator when the target position of the tool is at the target position at the current time, and calculates the target position and the posture information of the tool 2 at the current time according to the joint angle information of the actuator;

the information application processor 1012 converts the track position and posture information expressed by the sensor coordinate system into track position and posture information expressed by the coordinate system to which the track is attached, then marks the track position and posture information in the coordinate system to which the track is attached, and stores the track position and posture information in the information storage area to form a track position and posture information set { Q, N };

the information application processor calls the target position and posture information of the tool 2 at the current moment in the positive kinematics module 2011, and determines the target position and posture of the tool 2 at the next moment according to the target position and posture information of the tool 2 at the current moment and the track position and posture information set { Q, N };

the inverse kinematics module 2012 calls the target position and posture information of the tool 2 at the next time in the information application processor 1012, and calculates the joint angle information of the actuator when the target position of the tool 2 is at the target position at the next time according to the target position and posture of the tool 2 at the next time.

EXAMPLE six

The present embodiment is different from the fifth embodiment in that: in this embodiment, on the basis of the reference point Ref-a, the current control reference point Ref-a is not used as the tracking target, but the control reference point Ref-a-next after the control interval time t is used as the tracking target. In the embodiment, a specific determination method for determining the target position of the tool at the next time according to the set { Q, N } of the trajectory position and posture information is as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by passing through a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; the method comprises the steps that a sensor detects a section profile of a track line in a detection section M-sense of the sensor at a Ref-A-next point, an intersection line of a track division surface M-sea and the detection section M-sense is calculated according to the section profile, the intersection line is expressed as a vector N-sea, the vector N-sea is a posture of a tool at a control reference point Ref-A-next, and position and posture information of the control reference point Ref-A-next is used as a target position and posture of the tool at the next moment; wherein t is the adjustment interval time of each position correction control of the tool, V is the advancing speed of the tool, and the tool at the current time position moves to the target position at the next time after the interval time t.

And in the fifth step, the actuator makes the target position of the tool move from the target position at the current moment to the target position at the next moment in a correction mode according to the position and posture information of the control reference point Ref-A-next.

The other steps of the trajectory tracking control method, the specific development of each step, and the trajectory tracking system for implementing the trajectory tracking control method are the same as those in the fifth embodiment, and are not described again.

EXAMPLE seven

The difference between the present embodiment and the fifth embodiment is: in the fourth step of determining the target, the target position of the tool at the next time is determined according to the track position information set { Q, N }, and then the position deviation information between the position of the tool at the current time and the target position of the tool at the next time is further obtained.

The applicability and the specific development of other steps of the trajectory tracking control method are the same as those of the fifth embodiment, and are not described again here.

The specific trajectory tracking control method in this embodiment includes the following steps:

the first step is as follows: detecting a track: acquiring original track information of a detection point on a track on the workpiece 1 through a sensor 3;

the second step: extracting track information: extracting position information of a detection point and attitude information of a track segmentation surface at the detection point from the original track information, and expressing the position information and the attitude information by a sensor coordinate system;

The conversion process of converting the position and orientation information expressed by the sensor coordinate system into the position and orientation information expressed by the coordinate system to which the trajectory is attached is as follows:

Q-t =（T-sensor）X（Q-s）

N-t =（T-sensor）X（N-s）

the vector Q-T represents the description of the target position at the next moment in a tool coordinate system, the vector Q-s represents the description of the target position at the next moment in a sensor coordinate system, and the T-sensor is a position posture transformation relation matrix between the sensor coordinate system and the tool coordinate system; the sensor detects the section profile of the track characteristic line in the detection section M-sense of the sensor at a Ref-A point, the intersection line of the track segmentation plane M-sea and the detection section M-sense can be calculated according to the section profile, and the intersection line is expressed as a vector N-sea; the vector N-s represents the description of the vector N-team in the sensor coordinate system, and the vector N-t represents the description of the vector N-team in the tool coordinate system;

Q-w =（Tt）X Q-t =（Tt）X（T-sensor）X（Q-s）

N-w =（Tt）X N-t =（Tt）X（T-sensor）X（N-s）

The fourth step: determining a target: determining the target position and attitude of the tool at the next moment according to a track position and attitude information set { Q, N }, and calculating the position and attitude deviation information between the target position and attitude of the tool at the current moment and the target of the tool at the next moment, wherein the position and attitude deviation information is the position deviation information at least comprising two effective components of delta x and delta y in six effective components of the target position and attitude of the tool at the current moment and the target of the tool at the next moment, namely the lateral deviation delta x, the forward deviation delta y, the height deviation delta z, the pitching angle deviation delta x, the lateral deviation angle deviation delta y and the forward direction angle deviation delta z, and the position deviation information and attitude information is the position deviation information at most comprising six effective components of delta x, delta y, delta z, delta x, delta y and delta z; the specific determination method for determining the target position and the attitude of the tool at the next moment according to the track position and attitude information set { Q, N } is as follows:

setting the target position of the tool at the current moment as a point P, making a plane perpendicular to a track characteristic line by passing the point P, taking the intersection point of the plane and the track characteristic line as a reference point Ref-A, taking the reference point Ref-A as a tracking target, and determining a tool coordinate system P-X-toolY-toolZ-tool positioned at the reference point Ref-A: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the reference point Ref-A as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; the sensor detects the section contour of a track line in a detection section M-sense of the sensor at a Ref-A point, an intersection line of a track division plane M-sea and the detection section M-sense is calculated according to the section contour, the intersection line is represented as a vector N-sea, the vector N-sea is the posture of the tool at a reference point Ref-A, and the position and posture information of the reference point Ref-A is used as the target position and posture of the tool at the next moment.

The specific solving process of the position and attitude deviation information is as follows: setting a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' at a target position P point at the current moment,

δ x is: the trajectory characteristic line at the reference point Ref-A is linearized or passes through the reference point Ref-A to be taken as a tangent line of the trajectory characteristic line, and the trajectory characteristic line is linearized or the tangent line of the trajectory characteristic line forms an included angle between a projection line segment S-yz and a Y-tool ' axis on a plane passing through the Y-tool ' axis and the Z-tool ' axis;

The fifth step: a driving tool: and the actuator enables the target position of the tool to move from the target position at the current moment to the target position at the next moment according to the reference point Ref-Ade position and the attitude deviation information. The correction driving information for moving the target position of the tool from the current target position to the next target position is driving information D-tool, and the driving information D-tool at least comprises two effective components: Δ x, δ y, and the driving information D-tool is a vector containing at most six significant components: vectors of Δ x, Δ y, Δ z, δ x, δ y, δ z.

The trajectory tracking system for realizing the trajectory tracking control method comprises a sensor 3, a tool and an actuator, wherein the tool is attached to the actuator, the sensor 3 is attached to a joint of the actuator or the sensor 3 is directly attached to the tool, the relative position between the sensor 3 and the tool has a determined geometric relationship, the trajectory tracking system further comprises an information detection processor 1011, an information application processor 1012, a forward kinematics module 2011 and an inverse kinematics module 2012, and the information application processor 1012 further comprises an information storage area;

the information application processor 1012 calls the target position and posture information of the tool 2 at the current time calculated in the positive kinematics module 2011, determines the target position and posture information of the tool 2 at the next time according to the target position and posture information of the tool 2 at the current time and the track position and posture information set { Q, N }, and calculates the position and posture deviation information between the target position of the tool 2 at the current time and the target position of the tool 2 at the next time;

the inverse kinematics module 2012 can call the position and posture deviation information between the target position of the tool 2 at the current time and the target position of the tool 2 at the next time in the information application processor 1012, and calculate the joint angle information of the actuator when the target position of the tool is at the target position at the next time according to the position and posture deviation information between the target position of the tool 2 at the current time and the target position of the tool 2 at the next time.

The track tracking system can be further divided into a sensing system 5 and an execution system 6, functions of the sensing system 5 and the execution system 6 are explicitly disclosed, and track tracking can be well realized only by frequently exchanging information. For description of aspects, sending information from the sensing system 5 to the execution system 6 is defined as "downloading", sending information from the execution system 6 to the sensing system 5 is defined as "uploading", and the specific division of the sensing system 5 and the execution system 6 in the trajectory tracking system is the same as the four division methods in the first embodiment, and the specific working method is also the same, which is not described herein again.

Example eight

The present embodiment is different from the seventh embodiment in that: in this embodiment, on the basis of the reference point Ref-a, the current control reference point Ref-a is not used as the tracking target, but the control reference point Ref-a-next after the control interval time t is used as the tracking target. The specific determination method for determining the target position of the tool at the next time according to the trajectory position and posture information set { Q, N } in the embodiment is as follows: taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the track characteristic line through a control reference point Ref-A-next, wherein the intersection line of the plane and the track division surface is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; the method comprises the steps that a sensor detects a section profile of a track line in a detection section M-sense of the sensor at a Ref-A-next point, an intersection line of a track division plane M-sea and the detection section M-sense is calculated according to the section profile, the intersection line is expressed as a vector N-sea, the vector N-sea is a gesture of a tool at a control reference point Ref-A-next, the position and gesture information of the control reference point Ref-A-next are used as the target position and gesture of the tool at the next moment, and the position and gesture deviation information between the position of the tool at the current moment and the target position of the tool at the next moment is calculated according to the position and gesture information of the control reference point Ref-A-next; wherein t is the adjustment interval time of each position correction control of the tool, V is the advancing speed of the tool, and the tool at the current moment position moves to the target position at the next moment after the interval time t;

the specific solving process of the position and attitude deviation information is as follows: setting a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' at a target position P at the current moment,

δ x is: linearizing a track characteristic line positioned at a control reference point Ref-A-next or drawing a tangent line of the track characteristic line passing through the control reference point Ref-A-next, wherein the trajectory characteristic line is linearized or the tangent line of the track characteristic line forms an included angle between a projection line segment S-yz and a Y-tool ' axis on a section plane passing through the Y-tool ' axis and the Z-tool ' axis;

And in the fifth step, the actuator moves the target position of the tool from the target position at the current moment to the target position at the next moment according to the position and the posture information of the control reference point Ref-A-next.

The other steps of the trajectory tracking control method, the specific development of each step, and the trajectory tracking system for implementing the trajectory tracking control method are the same as those in the seventh embodiment, and are not described again.

The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in any way, but any modifications or equivalent variations made in accordance with the technical spirit of the present invention are within the scope of the present invention as claimed.

The trajectory tracking systems described in the first, second, third and fourth embodiments have low requirements for the sensing system, and can achieve the purpose of trajectory tracking even if the sensing system has insufficient capability to provide the height deviation of the weld position and the workpiece inclination degree, and cannot obtain the trajectory cross-sectional shape perfectly or frequently and quickly.

For example, when the sensor in the sensing system adopts a 2D camera, and the sensing system can only provide left and right deviation information of the weld position, and cannot provide height deviation of the weld position and the workpiece inclination degree, P-s = [ Xs,0, zs ], wherein Zs can only be a reasonable position assuming no height deviation, but not a detected value, and is only a fixed value. Assuming that the sensing coordinate system is parallel to the Z-axis of the tool coordinate system, then N-s = [ 0,1].

Because δ x is due to the change in height Zs, δ x =0;

since δ y is due to N-s variation, δ y =0;

since δ z is due to the variation in Xs, δ z ≠ 0.

Therefore, transformation to the tool coordinate system, calculated deviation:

D-tool = [Xtool，0, 0，0，0，δztool ]

then, as long as the trajectory is in a plane perpendicular to the Z-tool, i.e. the trajectory height has been guaranteed by the device itself, it can still be accurately tracked. Similar sensing system performance deficiencies are shown in fig. 14 for tracing a flat curve without arc height deviation Zs with a cartesian manipulator and in fig. 15 for tracing a curve with negligible arc height variation on a surface of a cylinder of revolution.

The sensing system cannot frequently and quickly obtain the track section shape, and can only obtain the situation of one or a very limited few track section shapes, for example, a displacement sensor is adopted as a sensor in the sensing system, the displacement sensor is a device for measuring the distance from one point to the sensor, and a common laser displacement sensor is a typical representative thereof. Such a sensor is used to track a straight weld on a plane, but the position and attitude of the weld cannot be determined with sufficient accuracy due to inaccuracies in the welding jig. By adopting the method, accurate tracking can still be carried out. This is done by attaching the sensor to the torch in a known geometric relationship to the torch. When the welding gun sweeps over the welding seam with the welding gun, the welding seam position on a section can be obtained at the starting point of the welding seam. Then there is a Q1-s and a constant N1-s, and welding can be performed in a constant attitude as long as Tt is adjusted at the starting point, assuming that the advancing direction of the weld is known. If the advancing direction of the weld is unknown, another cross-sectional scan can be performed at or near the end of the weld to obtain its information Q2-s, N2-s. Where N2-s = N1-s, since the straight weld trajectory pose is unchanged. The direction of the Y-tool axis is determined by Q2-s and Q1-s, and further the Tt of the starting point is determined by the direction of the Y-tool axis and the vector of N1-s. Then, tracking can be accomplished in two steps:

the first step is as follows: the welding torch is adjusted to Tt,

the second step is that: each cycle reaches the end point with the same drive information D-tool = [ X0,0 ].

As in the two examples, the elements in Q-s, N-s and D-tool can be supplemented by analysis according to practical situations, and then the tracking method provided by the invention can be applied, which is very meaningful for simplifying the execution equipment. The following rules are adopted:

as long as the delta z =0 in the D-tool, the curve track on a plane or an expandable curved surface (such as a rotary conical surface and a rotary cylindrical surface) can be tracked, and for a workpiece, equipment such as a positioner and the like is used for converting the workpiece into the expandable curved surface as much as possible; if Δ z ≠ 0, but with actuator limited to δ x =0, it is still possible to track with slowly undulating trajectories; if N-s is not a constant value, but the actuator is limited to delta y =0, the track with small amplitude lateral deflection change can be tracked; if a parameter cannot be detected, the actuator is configured to ensure that the change does not exceed a specified value.

The trajectory tracking system described in the first to eighth embodiments has low requirements on the execution system, and the trajectory tracking method described in the present invention can be used even if the execution system is an under-degree-of-freedom system.

Fig. 16 shows a case where the actuator is a 6-degree-of-freedom robot, the sensor is attached to the 6 th joint, and the welding gun is attached to the 5 th joint. It is also possible that the weld gun is actually mounted on the joint 6, but the change of the joint 6 has no effect on the weld gun feature point P, but the Y-axis of the sensor coordinate system is required to be offset by an angle θ from the Y-tool axis of the tool coordinate system to detect the weld. That is, the relative relationship between the sensor coordinate system and the tool coordinate system is changed during tracking. In this case, it is only necessary to distinguish the systems used for transforming the coordinate system, and Tt is not used in a general manner. This example also reveals one feature of the welding gun, and some glue guns: the posture of the tool does not need to be completely adjusted, so that an actuator with 5 degrees of freedom and one rotation degree of freedom can be used for tracking any spatial track, and the track in the tracking process only needs to be ensured to fall within the detection range of the sensor all the time. The embodiment is an example which can well expand the detection range of the sensing system.

When the execution system has less than 6 degrees of freedom, the execution system is lack of degrees of freedom, theoretically, an arbitrary curve cannot be accurately tracked, but the improved method can be analogized to be used for specific conditions according to the control principle, and the method belongs to the protection scope of the patent protection. For example, the above-mentioned situation that the 5-degree-of-freedom actuator drives the welding gun to track any spatial trajectory. For example, with actuators having only two or three translational degrees of freedom, it is possible to track curves on a plane. FIG. 17 illustrates a case where a track on a tilted plane of a tilted flexure is capable of being tracked; if the actuator has no freedom of lifting adjustment, the actuator can still track the track on a horizontally placed plane. For example, as shown in fig. 18, a two-degree-of-freedom actuator is provided with a horizontally-reversed positioner, and can track a curve on a revolving body. For example, as shown in fig. 19, an actuator with three translational degrees of freedom, a positioner with a vertical axis of rotation, a circular line that is eccentric or irregular enough to track on a table, etc. A positioner is a device that moves a workpiece relative to a robot arm. These less-than-free actuators can also track more complex trajectories if small gun attitude deviations are ignored, to name a few.

The problem of the contribution of the control system itself to the trajectory tracking in the foregoing embodiment 1 can also be reflected here. For example, when the forward drive component Δ y =0 in the D-tool, it is necessary to perform, for example, a "teaching trajectory" or a "planning trajectory" in the system and to complement at least one component Δ y ≠ 0.

Certainly, similar deviation information combination is not limited to the combination between the two sets of deviation information, and a plurality of deviation information sources can be provided, and the expression reference coordinate systems of the deviation information sources can also be different, namely the deviation information can be unified into the same reference system through conversion, and can also be corrected step by step under different reference systems respectively, and finally, a comprehensive control effect is obtained.

The control method provided by the invention is not particularly limited for the sensing system 5 and the executing system 6, but the systems can be coordinated with each other, and corresponding information adjustment is required according to respective unique functions, for example, parameters which cannot be sensed or controlled are defined as standard values reflecting actual conditions, so that the control method can be widely applied. The track tracking system provided by the invention can support various different sensing systems 5 and execution systems 6, and as long as the tracking system is established according to the rule of information exchange, devices which are as simple as possible or even incomplete can be selected to complete abundant specific tasks; and a complete sensing system 5 can be matched with an execution system 6 with full freedom or redundant freedom to complete a very complex tracking task. In addition, the track tracking system is further divided into a sensing system and an execution system, so that the tracking system can be established more conveniently and flexibly, and can be conveniently manufactured by different professional manufacturers, the performance is improved, and the cost is reduced.

Claims

1. The trajectory tracking control method is characterized by comprising the following steps: the tool is attached to the actuator, the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship, and the method specifically comprises the following steps:

(2) Extracting track information: extracting position information of a detection point from the original track information, and expressing the position information by a sensor coordinate system;

(4) Determining a target: and determining the target position of the tool at the next moment according to the track position information set { Q }, which is as follows:

setting a target position of a tool at the current moment as a point P, wherein a tool coordinate system of the point P is P-X-tool ' Y-tool ' Z-tool ', a plane perpendicular to a track characteristic line is formed by crossing the point P, an intersection point of the plane and the track characteristic line is a reference point Ref-A, the reference point Ref-A is taken as a tracking target, and the position of the reference point Ref-A is taken as a target position of the tool at the next moment;

(5) Driving a tool: the actuator moves the target position of the tool from the target position at the current moment to a Ref-A point according to the position information of a reference point Ref-A, and moves forward by a step delta = V x t along the positive direction of the Y-tool' axis by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool;

2. The trajectory tracking control method according to claim 1, characterized in that: in the step (4), the target position and the posture of the tool at the next moment are determined according to the track position information set { Q }, which is specifically as follows:

the characteristic that the surface of a workpiece on which the track is located faces in which direction at a detection point is a gesture, a target position of a tool at the current moment is set as a point P, a tool coordinate system of the point P is P-X-tool ' Y-tool ' Z-tool ', a plane perpendicular to the track characteristic line is made through the point P, an intersection point of the plane and the track characteristic line is a reference point Ref-A, the reference point Ref-A is taken as a tracking target, and the tool coordinate system P-X-tool Z-tool at the reference point Ref-A is determined: making a tangent line of the track characteristic line by passing through a reference point Ref-A, and taking the tangent line of the track characteristic line and a plane passing through a straight line parallel to the axis of the Z-tool' as a track segmentation plane; a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersecting line of the plane and the track segmenting surface is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and the posture of the reference point Ref-A as the target position and the posture of the tool at the next moment;

and (5) enabling the actuator to move the target position of the tool from the target position at the current moment to a point Ref-A according to the position and posture information of the reference point Ref-A, and moving forward by a step distance delta = V x t along the positive direction of the Y-tool axis by taking Ref-A as a starting point.

3. The trajectory tracking control method according to claim 1, characterized in that: in the step (2), a vector with the same direction as the axis direction of the Z-tool' is used as attitude information of a detection point, and the attitude information is expressed by a sensor coordinate system;

converting the attitude information expressed by the sensor coordinate system into attitude information expressed by a coordinate system attached to the track, marking the attitude information in the coordinate system attached to the track, and storing the attitude information in an information storage area to form a track attitude information set { N }, wherein the track attitude information set and the track position information set jointly form a track position attitude information set { Q, N };

in the step (4), according to the track position and posture information set { Q, N }, determining a tool coordinate system P-X-toolY-toolZ-tool located at the reference point Ref-A: a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersection line of the plane and the track dividing plane is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment;

4. A trajectory tracking control method according to claim 2 or 3, characterized in that: the specific determination method for determining the target position and posture of the tool at the next moment according to the trajectory position information set { Q } in the step (4) is as follows:

taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the track characteristic line through a control reference point Ref-A-next, wherein the intersection line of the plane and the track division surface is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as an axis Y-tool, wherein the positive direction of the axis Y-tool points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment;

and (5) enabling the target position of the tool to move from the target position at the current moment to the target position at the next moment by the actuator according to the position and the posture information of the control reference point Ref-A-next.

5. The track tracking control method is characterized by comprising the following steps: the tool is attached to the actuator, the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship, and the method specifically comprises the following steps:

(3) Marking track information: converting the position information expressed by the sensor coordinate system into position information expressed by a coordinate system attached to the track, marking the position information in the coordinate system attached to the track, and storing the position information in an information storage area to form a track position information set { Q }; when the position of the workpiece is not fixed, the coordinate system attached to the track is a workpiece coordinate system, and when the position of the workpiece is fixed, the coordinate system attached to the track is one of the workpiece coordinate system or a world coordinate system;

(4) Determining a target: determining the target position and the attitude of the tool at the next moment according to a track position information set { Q }, and calculating the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment, wherein the position and attitude deviation information is lateral deviation delta x, or the lateral deviation delta x is combined with at least one of advancing deviation delta y, height deviation delta z, pitching angle deviation delta x and advancing direction angle deviation delta z; the specific determination method for determining the target position and the attitude of the tool at the next moment according to the trajectory position information set { Q } is as follows:

setting a target position of a tool at the current moment as a point P, wherein a tool coordinate system of the point P is P-X-tool ' Y-tool ' Z-tool ', a plane perpendicular to a track characteristic line is formed through the point P, an intersection point of the plane and the track characteristic line is a reference point Ref-A, the reference point Ref-A is taken as a tracking target, and a tool coordinate system P-X-tool Y-tool Z-tool located at the reference point Ref-A is determined: making a tangent line of the track characteristic line by passing through a reference point Ref-A, and taking the tangent line of the track characteristic line and a plane passing through a straight line parallel to the axis of the Z-tool' as a track segmentation plane; a plane perpendicular to the characteristic line of the track is made through a reference point Ref-A, the intersecting line of the plane and the track segmenting surface is a Z-tool axis, and the positive direction of the Z-tool axis points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through the Ref-A point as a Y-tool axis, wherein the positive direction of the Y-tool axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the reference point Ref-A as the target position and posture of the tool at the next moment;

(5) Driving a tool: the actuator enables the target position of the tool to move from the target position at the current moment to a Ref-A point according to the position and attitude deviation information of the reference point Ref-A, and moves forwards by a step delta = V t along a preset direction by taking Ref-A as a starting point, wherein t is the adjustment interval time of each position correction control of the tool, and V is the advancing speed of the tool;

the method comprises the steps that corrected driving information for enabling a target position of a tool to move to a Ref-A point from a target position at the current moment is driving information D-tool, or the corrected driving information is the combination of the driving information D-tool and preset task track information, or the corrected driving information is the combination of the driving information D-tool and intervention information, or the corrected driving information is the combination of the driving information D-tool and the preset task track information and the intervention information; the driving information D-tool at least comprises one effective component: a vector of Δ x, and the driving information D-tool is a vector containing at most five significant components: vectors of Δ x, Δ y, Δ z, δ x, δ z;

wherein the predetermined direction is a direction that is naturally determined due to the displacement adjustment when the drive information D-tool does not contain δ x or δ z; when the driving information D-tool comprises deltax or deltaz, the preset direction is the positive direction of the Y-tool axis;

6. The trajectory tracking control method according to claim 5, characterized in that: the concrete solving process of the position and attitude deviation information in the step (4) is as follows: setting the tool coordinate system of the target position P point at the current moment as P-X-tool ' Y-tool ' Z-tool ',

7. The trajectory tracking control method according to claim 5, characterized in that: the specific determination method for determining the target position and posture of the tool at the next moment according to the trajectory position information set { Q } in the step (4) is as follows:

taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the track characteristic line through a control reference point Ref-A-next, wherein the intersection line of the plane and the track division surface is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining the X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment; and according to the position and attitude information of the control reference point Ref-A-next, calculating the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment, wherein the position and attitude deviation information is that the lateral deviation delta x or the lateral deviation delta x is combined with at least one of the advancing deviation delta y, the height deviation delta z, the pitching angle deviation delta x and the advancing direction angle deviation delta z;

8. The trajectory tracking control method according to claim 7, characterized in that: the specific solving process of the position and attitude deviation information in the step (4) is as follows: setting the tool coordinate system of the target position P point at the current moment as P-X-tool ' Y-tool ' Z-tool ',

δ x is: an included angle between the projection S-yz of a tangent line positioned at the position of a control reference point Ref-A-next on the track characteristic line in a plane determined by an axis Y-tool ' and an axis Z-tool ' and the direction of the axis Y-tool ' is formed;

9. The track tracking control method is characterized in that a sensor can sense position information and posture of a track, and the method comprises the following steps: the tool is attached to the actuator, the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship, and the method specifically comprises the following steps:

(3) Marking track information: converting the position and posture information expressed by the sensor coordinate system into position and posture information expressed by a coordinate system attached to the track, marking the position and posture information in the coordinate system attached to the track, and storing the position and posture information in an information storage area to form a track position and posture information set { Q, N }; when the position of the workpiece is not fixed, the coordinate system attached to the track is a workpiece coordinate system, and when the position of the workpiece is fixed, the coordinate system attached to the track is one of the workpiece coordinate system and a world coordinate system;

10. The trajectory tracking control method according to claim 9, wherein: in the step (4), a specific determination mode for determining the target position and the posture of the tool at the next moment according to the track position and posture information set { Q, N } is as follows:

taking a reference point Ref-A as a sphere center, taking the triangle = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by passing through a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment;

11. The trajectory tracking control method according to claim 9 or 10, characterized in that: in the step (3), the conversion process of converting the position and posture information expressed by the sensor coordinate system into the position and posture information expressed by the coordinate system attached to the track is as follows: the position and attitude information expressed by the sensor coordinate system is first converted into the position and attitude information expressed by the tool coordinate system, and then the position and attitude information expressed by the tool coordinate system is converted into the position and attitude information expressed by the coordinate system to which the track is attached.

12. The track tracking control method is characterized in that a sensor can sense position information and posture of a track, and the method comprises the following steps: the tool is attached to the actuator, the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship, and the method specifically comprises the following steps:

(4) Determining a target: determining a target position of the tool at the next moment according to a track position and posture information set { Q, N }, and calculating position and posture deviation information between the target position and posture of the tool at the current moment and a target of the tool at the next moment, wherein the position and posture deviation information is position deviation information which at least contains two effective components of delta x and delta y in six effective components of lateral deviation delta x, advancing deviation delta y, height deviation delta z, pitching angle deviation delta x, lateral deviation angle deviation delta y and advancing direction angle deviation delta z, and the position and posture deviation information is position deviation information which at most contains the six effective components of delta x, delta y, delta z, delta x, delta y and delta z; the specific determination method for determining the target position and posture of the tool at the next moment according to the track position and posture information set { Q, N } is as follows:

(5) Driving a tool: the actuator enables the target position of the tool to move from the target position at the current moment to the target position at the next moment according to the reference point Ref-A position and the attitude deviation information;

the method comprises the steps that corrected driving information enabling a target position of a tool to move from a current target position to a next target position is driving information D-tool, or the corrected driving information is the combination of the driving information D-tool and preset task track information, or the corrected driving information is the combination of the driving information D-tool and intervention information, or the corrected driving information is the combination of the driving information D-tool and the preset task track information and the intervention information; the driving information D-tool at least comprises two effective components: Δ x, δ y, and the driving information D-tool is a vector containing at most six significant components: vectors of Δ x, Δ y, Δ z, δ x, δ y, δ z;

13. The trajectory tracking control method according to claim 12, characterized in that: the concrete solving process of the position and attitude deviation information in the step (4) is as follows: setting a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' at a target position P at the current moment,

14. The trajectory tracking control method according to claim 12, wherein: in the step (4), a specific determination mode for determining the target position and the posture of the tool at the next moment according to the track position and posture information set { Q, N } is as follows:

taking a reference point Ref-A as a sphere center, taking delta = V X t as a radius to make a spherical surface, wherein the spherical surface and a track characteristic line have two intersection points, taking the intersection point of the spherical surface and the track characteristic line positioned in the advancing direction of the tool as a control reference point Ref-A-next, and taking the control reference point Ref-A-next as a tracking target to determine a tool coordinate system P-X-tool ' Y-tool ' Z-tool ' positioned at the control reference point Ref-A-next: making a plane perpendicular to the characteristic line of the track by passing through a control reference point Ref-A-next, wherein the intersection line of the plane and the track segmentation plane is a Z-tool shaft, and the positive direction of the Z-tool shaft points to the surface of the workpiece; taking a tangent line of a track characteristic line passing through a control reference point Ref-A-next as a Y-tool 'axis, wherein the positive direction of the Y-tool' axis points to the advancing direction of the tool; determining an X-tool axis and the positive direction of the X-tool axis according to the left-hand rule or the right-hand rule of the coordinate system; taking the position and posture information of the control reference point Ref-A-next as the target position and posture of the tool at the next moment, and calculating the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment according to the position and posture information of the control reference point Ref-A-next; wherein t is the adjustment interval time of each position correction control of the tool, V is the advancing speed of the tool, and the tool at the current time position moves to the target position at the next time after the interval time t;

15. The trajectory tracking control method according to claim 14, characterized in that: the specific solving process of the position and attitude deviation information in the step (4) is as follows: setting the tool coordinate system of the target position P point of the tool at the current moment as P-X-tool ' Y-tool ' Z-tool ',

δ y is: linearizing a track characteristic line positioned at a control reference point Ref-A-next or drawing a tangent line of the track characteristic line passing through the control reference point Ref-A-next, wherein the trajectory characteristic line is linearized or the tangent line of the track characteristic line forms an included angle between a projection line segment S-xz and a Z-tool ' axis on a section plane passing through an X-tool ' axis and the Z-tool ' axis;

16. The trajectory tracking control method according to claim 12, 13 or 14, characterized in that: in the step (3), the conversion process of converting the position and posture information expressed by the sensor coordinate system into the position and posture information expressed by the coordinate system attached to the track is as follows: the position and posture information expressed by the sensor coordinate system is converted into the position and posture information expressed by the tool coordinate system, and then the position and posture information expressed by the tool coordinate system is converted into the position and posture information expressed by the coordinate system attached to the track.

17. A trajectory tracking system for implementing the trajectory tracking control method according to any one of claims 1 to 4, comprising a sensor, a tool and an actuator, the tool being attached to the actuator, characterized in that: the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

the inverse kinematics module calls the target position and the attitude information of the tool calculated in the information application processor at the next moment, and calculates the joint angle information of the actuator when the target position of the tool is located at the target position at the next moment according to the target position and the attitude information of the tool at the next moment.

18. The trajectory tracking system of claim 17, wherein: the sensor and the information detection processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the information application processor, the forward kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system downloads position information of the trajectory expressed in the sensor coordinate system to the execution system.

19. The trajectory tracking system of claim 17, wherein: the sensor, the information detection processor and the information application processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the positive kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives target position and attitude information of the tool at the current moment uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

20. The trajectory tracking system of claim 17, wherein: the sensor, the information detection processor, the information application processor and the positive kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives the joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

21. The trajectory tracking system of claim 17, wherein: the sensor, the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the actuator forms an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives the joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the joint angle information of the actuator when the tool reaches the target position and the posture at the next moment to the execution system.

22. A trajectory tracking system for implementing the trajectory tracking control method according to any one of claims 5 to 8, comprising a sensor, a tool and an actuator, the tool being attached to the actuator, characterized in that: the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

the information application processor calls the target position and the attitude information of the tool at the current moment calculated in the positive kinematics module, determines the target position and the attitude information of the tool at the next moment according to the target position and the attitude information of the tool at the current moment and a track position information set { Q }, and calculates the position and attitude deviation information between the target position of the tool at the current moment and the target of the tool at the next moment;

the inverse kinematics module calls position and posture deviation information between a target position of the tool at the current moment and a target position of the tool at the next moment, which is calculated in the information application processor, and calculates joint angle information of the actuator when the target position of the tool is located at the target position at the next moment according to the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment.

23. The trajectory tracking system of claim 22, wherein: the sensor and the information detection processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the information application processor, the forward kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system downloads position information of the trajectory expressed in the sensor coordinate system to the execution system.

24. The trajectory tracking system of claim 22, wherein: the sensor, the information detection processor and the information application processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the positive kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system is electrically connected with the execution system through the first information interface end and the second information interface end; the sensing system receives target position and attitude information of the tool at the current moment uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

25. The trajectory tracking system of claim 22, wherein: the sensor, the information detection processor, the information application processor and the positive kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

26. The trajectory tracking system of claim 22, wherein: the sensor, the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the actuator forms an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives the joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the joint angle information of the actuator when the tool reaches the target position and the posture at the next moment to the execution system.

27. A trajectory tracking system for implementing the trajectory tracking control method according to any one of claims 9 to 11, comprising a sensor, a tool, and an actuator, the sensor being capable of sensing position information and attitude of the trajectory, the tool being attached to the actuator, characterized in that: the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

28. The trajectory tracking system of claim 27, wherein: the sensor and the information detection processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the information application processor, the forward kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system downloads position information of the trajectory expressed in the sensor coordinate system to the execution system.

29. The trajectory tracking system of claim 27, wherein: the sensor, the information detection processor and the information application processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the positive kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system is electrically connected with the execution system through the first information interface end and the second information interface end; the sensing system receives target position and attitude information of the tool at the current moment uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

30. The trajectory tracking system of claim 27, wherein: the sensor, the information detection processor, the information application processor and the positive kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

31. The trajectory tracking system of claim 27, wherein: the sensor, the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the actuator forms an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system is electrically connected with the execution system through the first information interface end and the second information interface end; the sensing system receives joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the joint angle information of the actuator when the tool reaches the target position and the posture at the next moment to the execution system.

32. A trajectory tracking system for implementing the trajectory tracking control method according to any one of claims 12 to 16, including a sensor, a tool, and an actuator, the sensor being capable of sensing position information and attitude of the trajectory, the tool being attached to the actuator, characterized in that: the sensor is attached to the joint of the actuator or the sensor is directly attached to the tool, and the relative position between the sensor and the tool has a determined geometric relationship; the system also comprises an information detection processor, an information application processor, a forward kinematics module and an inverse kinematics module, wherein the information application processor also comprises an information storage area;

the information application processor calls the position and posture information of the tool at the current moment calculated in the positive kinematics module, converts the position and posture information of the track expressed by the sensor coordinate system into the position and posture information expressed by the coordinate system attached to the track, marks the position and posture information in the coordinate system attached to the track, and stores the position and posture information in the information storage area to form a track position posture information set { Q, N };

the information application processor determines the target position and the attitude of the tool at the next moment according to the target position and the attitude information of the tool at the current moment and the track position and attitude information set { Q, N }, and obtains the position and attitude deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment;

the inverse kinematics module calls the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment calculated in the information application processor, and calculates the joint angle information of the actuator when the target position of the tool is located at the target position at the next moment according to the position and posture deviation information between the target position of the tool at the current moment and the target position of the tool at the next moment.

33. The trajectory tracking system of claim 32, wherein: the sensor and the information detection processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the information application processor, the forward kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system downloads position information of the trajectory expressed in the sensor coordinate system to the execution system.

34. The trajectory tracking system of claim 32, wherein: the sensor, the information detection processor and the information application processor form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the positive kinematics module, the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives target position and attitude information of the tool at the current moment uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

35. The trajectory tracking system of claim 32, wherein: the sensor, the information detection processor, the information application processor and the positive kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the inverse kinematics module and the actuator form an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives joint angle information of the actuator at the current target position of the tool uploaded by the execution system; and the sensing system downloads the target position and posture information of the tool at the next moment to the execution system.

36. The trajectory tracking system of claim 32, wherein: the sensor, the information detection processor, the information application processor, the forward kinematics module and the inverse kinematics module form a sensing system, and a first external communicator and a first information interface end are arranged in the sensing system; the actuator forms an execution system, a second external communicator and a second information interface end are arranged in the execution system, and the sensing system and the execution system are electrically connected through the first information interface end and the second information interface end; the sensing system receives joint angle information of the actuator at the target position of the tool at the current moment, which is uploaded by the execution system; and the sensing system downloads the joint angle information of the actuator when the tool reaches the target position and the posture at the next moment to the execution system.