CN110450150A

CN110450150A - Trajectory Tracking Control method and Trajectory Tracking System

Info

Publication number: CN110450150A
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: 2019-11-15
Anticipated expiration: 2038-05-07
Also published as: CN110450150B

Abstract

The invention discloses four kinds of Trajectory Tracking Control methods and corresponding Trajectory Tracking Systems, the basic thought of four kinds of Trajectory Tracking Control methods is all consistent, it is all the trace information immobilization that the sensor coordinate system with variation that first sensor is obtained describes, it is converted into after sensing and the form of the coordinate system description of whole recording track information is suitble to be marked, true track is gradually presented, basis is provided for subsequent solution target, perfect condition that current time tool should be presented finally found according to target or finds after this amendment of completion the state to be achieved when the amendment moment arrives next time, then actuator is driven according to above- mentioned information, the track of amendment tool.Above-mentioned four kinds of Trajectory Tracking Control methods and corresponding Trajectory Tracking System, the advanced bring error of test point is overcome, teaching information independent operating can be detached from, the position deviation of tool can be corrected, the attitude misalignment of tool can be corrected again, and track following effect is good.

Description

Trajectory Tracking Control method and Trajectory Tracking System

Technical field

The present invention relates to track automatic tracking technology field more particularly to Trajectory Tracking Control method and realize the track The Trajectory Tracking System of tracking and controlling method.

Background technique

The operations such as welding, gluing, cutting are a kind of similar routine operations, and common feature is along specific track It is operated, if the specific track position can be detected using sensor, so that it may with actuators such as robots to track It is tracked, to realize automated job.

Precursor in existing Trajectory Tracking System is adopted using sensor come pursuit path, such as welding robot system With structured light vision sensor come pursuit path.Actuator itself in Trajectory Tracking System has one to pass through teaching information or rule Activation bit can be generated according to the scheduled task track, make work by the scheduled task track of means, the actuator with tool such as drawing Tool is run along scheduled task track, and Trajectory Tracking System is only capable of the trajector deviation that simply sensor is acquired, is extracted Information is directly fed back in actuator, is modified to scheduled task track.Since sensor is generally located on actuator Front end, so sensor detection position and tool location are inconsistent during track following, and it is existing for track following Sensor, be only capable of providing its trajector deviation information relative to its own detected to external, in the tracking control system The Trajectory Tracking Control method of use has ignored sensor detection position and the inconsistent problem of tool location, roughly will sensing The position deviation that device detects is directly used in the correction to tool location.This method is that the track that is detected based on sensor is inclined Poor information, it is unbiased not entangle, even if then means current location does not have deviation, after advancing according to scheduled task track, lower a period of time Deviation can also be generated by carving tool location, so theoretically cannot achieve whole agonic tracking, deviation can only be reduced to not New deviation caused by greater than often taking a step forward, consequently only that the radius of curvature in track is larger or the lesser feelings of fluctuation of track Track following purpose could be preferably realized under condition, but the radius of curvature in track is smaller or the biggish feelings of fluctuation of track Track following is carried out under condition will generate biggish track following error, or even be unable to complete track following purpose.It sets up at present Tracking system is cumbersome, and for track, especially space curve track, there is no general Trajectory Tracking Control method and realization The Trajectory Tracking System of Trajectory Tracking Control method.

Summary of the invention

The operations such as welding, gluing, cutting are a kind of similar routine operations, and common feature is along specific track It is operated, it is the mesh tracked when tool works in the ideal situation that track, which is any line segment that is curved, having " posture ", Workpiece where perception track is here table by mark, characteristic of the workpiece surface at test point " towards which direction " where track Characteristic of the face at test point " towards which direction " is known as " posture " of track, and above-mentioned specific track can be abstracted into one Straightway or curved section, this straightway or curved section are exactly the Trajectory Tracking System track to be tracked, and are claimed here For track characteristic line.

The technical issues of solving needed for of the invention is: provide it is a kind of it is general, teaching information independent operating can be detached from, can be real It now automatically tracks and Trajectory Tracking Control method and Trajectory Tracking System that tracking effect is good.

To solve the above problems, the first trace tracking method of the present invention is to detect track in advance using sensor The characteristics of, using the method for coordinate transform, the track original position-information that sensor detects is tagged to workpiece coordinate system or generation In boundary's coordinate system, a track position information set { Q } being made of multiple test points is formed, then according to track position information Set { Q } determines the correct position and posture that subsequent time tool should reach, and the method overcome the advanced brings of test point Error.

The first Trajectory Tracking Control method of the present invention, tool are attached on actuator, and sensor, which is attached to, to be held On the joint of row device or sensor is attached directly on tool, and the relative position between sensor and tool has the several of determination What relationship, specifically includes the following steps:

(1) it detects track: acquiring the initial trace information of test point on the track on workpiece by sensor；

(2) it extracts trace information: extracting the location information of test point, and from initial trace information with sensor coordinate system table Up to the location information；

(3) it marks trace information: converting the location information expressed with sensor coordinate system to coordinate system accompanying by track Then the location information of expression is marked in coordinate system accompanying by track, and be stored in information storage area and form track position Set information aggregate { Q }；Coordinate system accompanying by track is workpiece coordinate system when wherein the location of workpiece is not fixed, and the location of workpiece is fixed When track accompanying by coordinate system be one of workpiece coordinate system or world coordinate system；

(4) determine target: determined according to track position information set { Q } tool in the target position of subsequent time, specifically such as Under: tool is set in the target position at current time as P point, and the tool coordinates system of P point is P-X-tool ' Y-tool ' Z- Tool ' crosses P point and does the plane vertical with track characteristic line, and the intersection point of the plane and track characteristic line is reference point Ref-A, with Reference point Ref-A is tracking target, using the position of reference point Ref-A as tool in the target position of subsequent time；

(5) driving instrument: actuator according to the location information of reference point Ref-A, make the target position of tool by current time Target position moves to Ref-A point, and using Ref-A as starting point along the positive direction of Y-tool ' axis move forward a step pitch △= V*t, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool；

(6) above steps is recycled, until completing the tracking of whole track.

Further, the first Trajectory Tracking Control method above-mentioned, wherein according to track position information in step (4) Set { Q } determines target position and posture of the tool in subsequent time, specific as follows: setting tool in the target position at current time It is set to P point, the tool coordinates system of P point is P-X-tool ' Y-tool ' Z-tool ', and P point excessively is done vertical with track characteristic line The intersection point of plane, the plane and track characteristic line is reference point Ref-A, is tracking target with reference point Ref-A, determines and be located at ginseng Tool coordinates system P-X-toolY-toolZ-tool at examination point Ref-A: crossing the tangent line that reference point Ref-A makees track characteristic line, To cross the tangent line of track characteristic line and cross the plane of the straight line parallel with Z-tool ' axis as track divisional plane；Cross reference point Ref- A does the plane perpendicular to track characteristic line, and the intersection of the plane and track divisional plane is Z-tool axis, the positive direction of Z-tool axis It is directed toward workpiece surface；With the tangent line of the track characteristic line of Ref-A point excessively for Y-tool axis, the positive direction of Y-tool axis is directed toward tool Direction of advance；The positive direction of X-tool axis and X-tool axis is determined according to the left hand rule of coordinate system or the right-hand rule；It will refer to Target position and posture of the position and posture of point Ref-A as tool in subsequent time；At this time in step (5) actuator according to The position of reference point Ref-A and posture information make the target position of tool move to Ref-A by the target position at current time Point, and a step pitch △=V*t is moved forward along the positive direction of Y-tool axis using Ref-A as starting point.

Further, the first Trajectory Tracking Control method above-mentioned, wherein with one and Z-tool ' axis in step (2) Posture information of the identical vector in direction as test point, and the posture information is expressed with sensor coordinate system.

At this time convert the posture information expressed with sensor coordinate system to coordinate system accompanying by track in step (3) Then the posture information of expression is marked in coordinate system accompanying by track, and be stored in formation track appearance in information storage area State information aggregate { N }, track posture information set and track position information set collectively form track position posture information set { Q, N }.

At this time in step (4), according to track position posture information set { Q, N }, the work being located at reference point Ref-A is determined Tool coordinate system P-X-toolY-toolZ-tool: it crosses reference point Ref-A and does the plane perpendicular to track characteristic line, the plane and rail The intersection of mark divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface；To cross the track characteristic line of Ref-A point Tangent line be Y-tool axis, the positive direction of Y-tool axis is directed toward tool direction of advance；According to the left hand rule or the right hand of coordinate system Rule determines the positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture information as tool under The target position at one moment and posture.

At this time in step (5) actuator according to the position of reference point Ref-A and posture information, make the target position of tool by The target position at current time moves to Ref-A point, and moves forward one along the positive direction of Y-tool axis using Ref-A as starting point A step pitch △=V*t.

Further, the first Trajectory Tracking Control method above-mentioned, wherein according to track position information in step (4) Set { Q } come determine tool in the target position of subsequent time, it is specific as follows: using reference point Ref-A as the centre of sphere, to be with △=V*t Radius does spherical surface, and there are two intersection points for the spherical surface and track characteristic line, and fetch bit is in spherical surface and track characteristic in tool direction of advance The intersection point of line is control reference point Ref-A-next, to control reference point Ref-A-next as tracking target, determines and is located at control Tool coordinates system P-X-tool " Y-tool " Z-tool " at reference point Ref-A-next: control reference point Ref-A-next is crossed The plane perpendicular to track characteristic line is done, the intersection of the plane and track divisional plane is Z-tool " axis, the pros of Z-tool " axis To direction workpiece surface；It is Y-tool " axis, Y-tool " to cross the tangent line for the track characteristic line for controlling reference point Ref-A-next The positive direction of axis is directed toward tool direction of advance；X-tool " axis and X- are determined according to the left hand rule of coordinate system or the right-hand rule The positive direction of tool " axis；Using control reference point Ref-A-next position and posture information as tool subsequent time mesh Cursor position and posture；Actuator according to the position of control reference point Ref-A-next and posture information, makes work in step (5) at this time The target position of tool moves to subsequent time target position by the target position at current time.

Further, the first Trajectory Tracking Control method above-mentioned, wherein will be with sensor coordinate system in step (3) The location information of expression is converted into the conversion process of the location information of the expression of coordinate system accompanying by track are as follows: first will be with sensing The location information of device coordinate system expression is converted into the location information expressed with tool coordinates system, then will be expressed with tool coordinates system Location information be converted into coordinate system accompanying by track express location information.

Realize the Trajectory Tracking System of the first Trajectory Tracking Control method, including sensor, tool and actuator, tool Be attached on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor with Relative position between tool has determining geometrical relationship；It further include infomation detection processor, Information application processor, positive fortune It is dynamic to learn module and inverse kinematics module, it further include information storage area in Information application processor；

Sensor acquires the initial trace information of track on workpiece；

The initial trace information extraction that infomation detection processor is acquired according to sensor goes out the location information of track and with sensor Coordinate system expression；

Positive kinematics module obtains the joint angle information of target position actuator at current target position of tool, and root Target position and posture information of the tool at current time are calculated according to the joint angle information of the actuator；

Information application processor converts the location information for the track expressed with sensor coordinate system to seat accompanying by track The location information of mark system expression, is then marked in coordinate system accompanying by track, and be stored in information storage area and form rail Mark location information set { Q }；

Information application processor calls the target position at calculated tool current time and posture information in positive kinematics module, And determine tool next with posture information and track position information set { Q } according to the target position at tool current time The target position at moment and posture information；

Calculated tool is believed in the target position of subsequent time and posture in inverse kinematics module recalls information application processor Breath, and target position and posture information according to tool in subsequent time, the target position for calculating tool are located at subsequent time The joint angle information of actuator when target position.

It is prior the characteristics of detecting track that second of trace tracking method of the present invention, which is using sensor, using seat The method for marking transformation is tagged to the track original position-information that sensor detects in workpiece coordinate system or world coordinate system, shape The track position information set { Q } being made of at one multiple test points determines lower a period of time according to track position information set { Q } The correct position that should reach of quarter tool and posture, then further find out tool and exist in current target position and tool Position and attitude misalignment information between the target position of subsequent time change tool according to the position and attitude misalignment information and work as The target position at preceding moment, posture reach the correct position posture of subsequent time, then all falling after tool often takes a step forward In correct position in posture, it is achieved that theoretic agonic tracking effect, and the trace tracking method also overcomes Test point advanced bring error.

Second of trace tracking method of the present invention, tool are attached on actuator, and sensor is attached to actuator Joint on or sensor be attached directly on tool, and there is the relative position between sensor and tool determining geometry to close System, specifically includes the following steps:

(4) it determines target: determining target position and posture of the tool in subsequent time according to track position information set { Q }, And find out tool in position between subsequent time target position of current target position and tool and attitude misalignment information, The position and attitude misalignment information be lateral deviation △ x or lateral deviation △ x and progressive deviation △ y, height tolerance △ z, At least one of pitch angle deviation δ x, direction of advance angular deviation δ z are combined；Wherein according to track position information set { Q } determines that tool is as follows in the target position of subsequent time and the specific method of determination of posture: setting tool at current time Target position is P point, and the tool coordinates system of P point is P-X-tool ' Y-tool ' Z-tool ', crosses P point and does and track characteristic line The intersection point of vertical plane, the plane and track characteristic line is reference point Ref-A, is tracking target with reference point Ref-A, determines Tool coordinates system P-X-toolY-toolZ-tool at reference point Ref-A: it crosses reference point Ref-A and makees track characteristic line Tangent line, to cross the tangent line of track characteristic line and the plane of the mistake straight line parallel with Z-tool ' axis as track divisional plane；Cross ginseng Examination point Ref-A does the plane perpendicular to track characteristic line, and the intersection of the plane and track divisional plane is Z-tool axis, Z-tool axis Positive direction be directed toward workpiece surface；With the tangent line of the track characteristic line of Ref-A point excessively for Y-tool axis, the positive direction of Y-tool axis Direction tool direction of advance；The pros of X-tool axis and X-tool axis are determined according to the left hand rule of coordinate system or the right-hand rule To；Target position and posture using the position of reference point Ref-A and posture information as tool in subsequent time；

(5) driving instrument: actuator according to the position of reference point Ref-A and attitude misalignment information, make the target position of tool by The target position at current time moves to Ref-A point, and moves forward a step pitch △ along predetermined direction using Ref-A as starting point =V*t, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool；Make the target of tool Position is activation bit D-tool, activation bit by the amendment activation bit that the target position at current time moves to Ref-A point D-tool is to contain at least one active constituent: the vector of △ x, and activation bit D-tool is at most to include five effectively to divide Amount: the vector of △ x, △ y, △ z, δ x, δ z；Wherein when activation bit D-tool does not include δ x or δ z, the predetermined direction is The naturally determined direction due to adjustment of displacement；When activation bit D-tool includes δ x or δ z, the predetermined direction is The positive direction of Y-tool axis；

(6) above steps is recycled, until completing the tracking of whole track.

Further, second of Trajectory Tracking Control method above-mentioned, wherein position and attitude misalignment letter in step (4) The specific solution procedure of breath are as follows: set the tool coordinates system of current target position P point as P-X-tool ' Y-tool ' Z- Tool ',

δ z is: being located at the tangent line at reference point Ref-A on track characteristic line in the plane for crossing X-tool ' axis and Y-tool ' axis Projection S-xy and Y-tool ' axis direction angle；

δ x is: being located at the tangent line at reference point Ref-A on track characteristic line in the plane for crossing Y-tool ' axis and Z-tool ' axis Projection S-yz and Y-tool ' axis direction angle；

△ x is: projected length of the vector of P point to reference point Ref-A in X-tool ' axis direction；

△ y is: projected length of the vector of P point to reference point Ref-A in Y-tool ' axis direction；

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

Further, second of Trajectory Tracking Control method above-mentioned, wherein according to track position information in step (4) Gather { Q } to determine that tool is as follows in the target position of subsequent time and the specific method of determination of posture: being with reference point Ref-A The centre of sphere does spherical surface by radius of △=V*t, and there are two intersection points for the spherical surface and track characteristic line, and fetch bit is in tool direction of advance The intersection point of spherical surface and track characteristic line is control reference point Ref-A-next, to control reference point Ref-A-next as tracking mesh Mark, the tool that determines are located at the tool coordinates system P-X-tool " Y-tool " Z-tool " at control reference point Ref-A-next: crossing and control Reference point Ref-A-next processed does the plane perpendicular to track characteristic line, and the intersection of the plane and track divisional plane is Z-tool The positive direction of axis, Z-tool " axis is directed toward workpiece surface；It is to cross the tangent line for the track characteristic line for controlling reference point Ref-A-next The positive direction of Y-tool " axis, Y-tool " axis is directed toward tool direction of advance；It is true according to the left hand rule of coordinate system or the right-hand rule Determine the positive direction of X-tool " axis and X-tool " axis；Using the position and posture information that control reference point Ref-A-next as tool Target position and posture in subsequent time；And tool is found out according to the position and posture information of control reference point Ref-A-next In position between subsequent time target position of current target position and tool and attitude misalignment information, the position And attitude misalignment information is lateral deviation △ x or lateral deviation △ x and progressive deviation △ y, height tolerance △ z, pitch angle are inclined At least one of poor δ x, direction of advance angular deviation δ z are combined.At this time in step (5) actuator according to control reference point The position of Ref-A-next and posture information make the target position of tool by the target position at current time to subsequent time target Position.

δ z is: being located at the tangent line at control reference point Ref-A-next on track characteristic line and is crossing X-tool ' axis and Y-tool ' The angle for projection S-xy and Y-tool ' axis direction in plane that axis determines；

δ x is: being located at the tangent line at control reference point Ref-A-next on track characteristic line and is crossing Y-tool ' axis and Z-tool ' The angle for projection S-yz and Y-tool ' axis direction in plane that axis determines；

△ x is: projected length of the vector of P point to control reference point Ref-A-next in X-tool ' axis direction；

△ y is: projected length of the vector of P point to control reference point Ref-A-next in Y-tool ' axis direction；

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

Further, second of Trajectory Tracking Control method above-mentioned, wherein the amendment activation bit in step (5) is The synthesis activation bit that activation bit D-tool is combined at least one of scheduled task trace information, intervention information.

Further, second of Trajectory Tracking Control method above-mentioned, wherein will be with sensor coordinate system in step (3) The location information of expression is converted into the conversion process of the location information of the expression of coordinate system accompanying by track are as follows: first will be with sensing The location information of device coordinate system expression is converted into the location information expressed with tool coordinates system, then will be expressed with tool coordinates system Location information be converted into coordinate system accompanying by track express location information.

Realize the Trajectory Tracking System of second of Trajectory Tracking Control method, including sensor, tool and actuator, tool Be attached on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor with Relative position between tool has determining geometrical relationship；It further include infomation detection processor, Information application processor, positive fortune It is dynamic to learn module and inverse kinematics module, it further include information storage area in Information application processor；

Sensor acquires the initial trace information of track on workpiece；

Information application processor calls the target position at calculated tool current time and posture information in positive kinematics module, And determine tool next with posture information and track position information set { Q } according to the target position at tool current time The target position at moment and posture information, and tool is found out in current target position and tool in the target position of subsequent time Position and attitude misalignment information between setting；

Calculated tool is in current target position and tool under in inverse kinematics module recalls information application processor Position and attitude misalignment information between the target position at one moment, and existed according to tool in current target position and tool Position and attitude misalignment information between the target position of subsequent time, the target position for calculating tool are located at subsequent time mesh Joint angle information when cursor position.

Track is any line segment that is curved, having " posture ", is the mesh tracked when tool works in the ideal situation Mark, wherein " posture " refers to when track following that the specified point on not only tool will be fallen on track characteristic line, and the axis of tool Line will be with track characteristic line relationship at an angle.The first above-mentioned Trajectory Tracking Control method and second of track following control Sensor involved in method processed and corresponding Trajectory Tracking System can only perceive target trajectory in the side for leaving tool Upward deviation, the deviation in two side directions of track, are collectively referred to as the location information of track, which can not perceive track Workpiece where perception track is here that surface is being detected by characteristic of the place workpiece surface at test point " towards which direction " Characteristic at point " towards which direction " is known as " posture " of track.The third trace tracking method of the present invention and correspondence Trajectory Tracking System involved in sensor can not only perceive deviation of the target trajectory on the direction for leaving tool, in-orbit Deviation in two side directions of mark, moreover it is possible to perceive the posture information of track.The third trace tracking method of the present invention is It is prior the characteristics of detecting track using sensor, using the method for coordinate transform, the track home position that sensor is detected And posture information is tagged in workpiece coordinate system or world coordinate system, forms a track position appearance being made of multiple test points State information aggregate { Q, N } then determines that subsequent time tool should reach just according to track position posture information set { Q, N } True target position and posture, the method overcome the advanced bring errors of test point.

The third trace tracking method of the present invention, sensor can perceive the location information and posture of track, tool Be attached on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor with Relative position between tool has determining geometrical relationship, specifically includes the following steps:

(2) trace information is extracted: from the location information and the track at the test point for extracting test point in initial trace information The posture information of divisional plane, and the location information and posture information are expressed with sensor coordinate system；

(3) it marks trace information: converting the position and posture information expressed with sensor coordinate system to accompanying by track The position of coordinate system expression and posture information, are then marked in coordinate system accompanying by track, and be stored in information storage area Middle formation track position posture information set { Q, N }；Coordinate system accompanying by track is workpiece when wherein the location of workpiece is not fixed Coordinate system, coordinate system accompanying by track is one of workpiece coordinate system or world coordinate system when the location of workpiece is fixed；

(4) determine target: determined according to track position posture information set { Q, N } tool in the target position of subsequent time and Posture, specific as follows:

If tool is P point in the target position at current time, crosses P point and do the plane vertical with track characteristic line, the plane and rail The intersection point of mark characteristic curve is reference point Ref-A, is tracking target with reference point Ref-A, determines the work being located at reference point Ref-A Tool coordinate system P-X-toolY-toolZ-tool: crossing reference point Ref-A and do the plane perpendicular to track characteristic line, the plane with The intersection of track divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface；To cross the track of reference point Ref-A The tangent line of characteristic curve is Y-tool axis, and the positive direction of Y-tool axis is directed toward tool direction of advance；According to the left hand rule of coordinate system Or the right-hand rule determines the positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture information as work Have the target position in subsequent time and posture；

(5) driving instrument: actuator according to the position of reference point Ref-A and posture information, make the target position of tool by current The target position at moment moves to Ref-A point, and moves forward a step along the positive direction of Y-tool axis using Ref-A as starting point Away from △=V*t, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool；

(6) above steps is recycled, until completing the tracking of whole track.

Further, the third Trajectory Tracking Control method above-mentioned, wherein according to track position posture in step (4) Information aggregate { Q, N } determines that tool is as follows in the target position of subsequent time and the specific method of determination of posture: with reference point Ref-A is the centre of sphere, does spherical surface by radius of △=V*t, and there are two intersection point, fetch bit is advanced in tool for the spherical surface and track characteristic line The intersection point of spherical surface and track characteristic line on direction is control reference point Ref-A-next, is to control reference point Ref-A-next Target is tracked, determines the tool coordinates system P-X-tool " Y-tool " Z-tool ": mistake being located at control reference point Ref-A-next Control reference point Ref-A-next does the plane perpendicular to track characteristic line, and the intersection of the plane and track divisional plane is Z- The positive direction of tool " axis, Z-tool " axis is directed toward workpiece surface；With the track characteristic line of control reference point Ref-A-next excessively Tangent line is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool direction of advance；According to the left hand rule or the right hand of coordinate system Rule determines the positive direction of X-tool " axis and X-tool " axis；The position for controlling reference point Ref-A-next and posture information are made Target position and posture for tool in subsequent time.At this time in step (5) actuator according to control reference point Ref-A-next Position and posture information, the target position of tool is made to move to the target position of subsequent time by the target position at current time It sets.

Further, the third Trajectory Tracking Control method above-mentioned, wherein will be with sensor coordinate system in step (3) The position of expression and posture information are converted into the conversion process with the position of the expression of coordinate system accompanying by track and posture information Are as follows: it first converts the position and posture information expressed with sensor coordinate system to and is believed with the position and posture of tool coordinates system expression Then breath converts the position and posture information expressed with tool coordinates system to the position expressed with coordinate system accompanying by track And posture information.

Realize the Trajectory Tracking System of the third Trajectory Tracking Control method, including sensor, tool and actuator, tool Be attached on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor with Relative position between tool has determining geometrical relationship；It further include infomation detection processor, Information application processor, positive fortune It is dynamic to learn module and inverse kinematics module, it further include information storage area in Information application processor；

Sensor acquires the initial trace information of track on workpiece；

The initial trace information extraction that infomation detection processor is acquired according to sensor go out track position and posture information and with Sensor coordinate system expression；

Information application processor converts the position for the track expressed with sensor coordinate system and posture information to appended by track Coordinate system expression position and posture information, be then marked in coordinate system accompanying by track, and be stored in information and deposit Track position posture information set { Q, N } is formed in storage area；

Information application processor calls the target position at calculated tool current time and posture information in positive kinematics module, And work is determined in the target position at current time and posture information and track position posture information set { Q, N } according to tool Have the target position in subsequent time and posture；

Calculated tool is believed in the target position of subsequent time and posture in inverse kinematics module recalls information application processor Breath, and target position and posture according to tool in subsequent time, the target position for calculating tool are located at subsequent time target The joint angle information of actuator when position.

Sensor involved in 4th kind of trace tracking method of the present invention and corresponding Trajectory Tracking System is not only Deviation of the target trajectory on the direction for leaving tool, the deviation in two side directions of track can be perceived, moreover it is possible to perceive rail The posture information of mark.It is prior the characteristics of detecting track that 4th kind of trace tracking method of the present invention, which is using sensor, Using the method for coordinate transform, the track home position of sensor detection and posture information are tagged to workpiece coordinate system or the world In coordinate system, a track position posture information set { Q } being made of multiple test points is formed, is believed according to track position posture Breath set { Q } determines the correct position and posture that subsequent time tool should reach, and then further finds out tool current Moment target position and position and attitude misalignment information of the tool between the target position of subsequent time, according to the position and appearance State deviation information change tool current target position, posture reach the correct position posture of subsequent time, then work It after tool often takes a step forward, all falls on correct position and attitude, is achieved that theoretic agonic tracking effect, and should Trace tracking method also overcomes the advanced bring error of test point.

4th kind of trace tracking method of the present invention, sensor can perceive the location information and posture of track, tool Be attached on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor with Relative position between tool has determining geometrical relationship, specifically includes the following steps:

(4) determine target: determined according to track position posture information set { Q, N } tool in the target position of subsequent time and Posture, and it is inclined in position between the target of subsequent time of current target position and attitude and tool and posture to find out tool Poor information, the position and attitude misalignment information are lateral deviation △ x, progressive deviation △ y, height tolerance △ z, pitch angle Deviation δ x, lateral deviation angular displacement δ y, at least x containing △ in direction of advance angular deviation δ z this six active constituents, δ y two is effectively point The position deviation information of amount, and it includes △ x, △ y, △ z, δ x, δ y, δ z six that the position and attitude misalignment information, which is at most, The position deviation information of active constituent；Wherein determine tool in lower a period of time according to according to track position posture information set { Q, N } The target position at quarter and the specific method of determination of posture are as follows: setting tool in the target position at current time as P point, cross P point and do The intersection point of the plane vertical with track characteristic line, the plane and track characteristic line is reference point Ref-A, is with reference point Ref-A Target is tracked, determines the tool coordinates system P-X-toolY-toolZ-tool being located at reference point Ref-A: crossing reference point Ref-A The plane perpendicular to track characteristic line is done, the intersection of the plane and track divisional plane is Z-tool axis, the positive direction of Z-tool axis It is directed toward workpiece surface；With the tangent line of the track characteristic line of reference point Ref-A excessively for Y-tool axis, the positive direction of Y-tool axis is directed toward Tool direction of advance；The positive direction of X-tool axis and X-tool axis is determined according to the left hand rule of coordinate system or the right-hand rule；It will Target position and posture of the position and posture information of reference point Ref-A as tool in subsequent time；

(5) driving instrument: actuator makes the target position of tool by working as according to the position reference point Ref-A and attitude misalignment information Preceding moment target position moves to the target position of subsequent time；Move the target position of tool by current target position Amendment activation bit to the target position of subsequent time is activation bit D-tool, and activation bit D-tool is including at least two A active constituent: the vector of △ x, δ y, and it includes six active constituents that activation bit D-tool, which is at most: △ x, △ y, △ z, δ The vector of x, δ y, δ z；

(6) above steps is recycled, until completing the tracking of whole track.

Further, the 4th kind of Trajectory Tracking Control method above-mentioned, wherein position and attitude misalignment letter in step (4) The specific solution procedure of breath are as follows: the tool coordinates system for being set in current target position P point is P-X-tool ' Y-tool ' Z-tool ',

△ z is: projected length of the vector of P point to reference point Ref-A in Z-tool ' axis direction；

δ x is: by be located at reference point Ref-A place track characteristic line linearization(-sation) or mistake reference point Ref-A make cutting for track characteristic line Projection line of the tangent line of line, track characteristic line linearization(-sation) or track characteristic line in the plane for crossing Y-tool ' axis and Z-tool ' axis The angle of section S-yz and Y-tool ' axis；

δ y is: by be located at reference point Ref-A place track characteristic line linearization(-sation) or mistake reference point Ref-A make cutting for track characteristic line Projection line of the tangent line of line, track characteristic line linearization(-sation) or track characteristic line in the plane for crossing X-tool ' axis and Z-tool ' axis The angle of section S-xz and Z-tool ' axis；

δ z is: by be located at reference point Ref-A place track characteristic line linearization(-sation) or mistake reference point Ref-A make cutting for track characteristic line Projection line of the tangent line of line, track characteristic line linearization(-sation) or track characteristic line in the plane for crossing X-tool ' axis and Y-tool ' axis The angle of section S-xy and Y-tool ' axis.

Further, the 4th kind of Trajectory Tracking Control method above-mentioned, wherein according to track position posture in step (4) Information aggregate { Q, N } determines that tool is as follows in the target position of subsequent time and the specific method of determination of posture: with reference point Ref-A is the centre of sphere, does spherical surface by radius of △=V*t, and there are two intersection point, fetch bit is advanced in tool for the spherical surface and track characteristic line The intersection point of spherical surface and track characteristic line on direction is control reference point Ref-A-next, is to control reference point Ref-A-next Target is tracked, determines the tool coordinates system P-X-tool " Y-tool " Z-tool ": mistake being located at control reference point Ref-A-next Control reference point Ref-A-next does the plane perpendicular to track characteristic line, and the intersection of the plane and track divisional plane is Z- The positive direction of tool " axis, Z-tool " axis is directed toward workpiece surface；With the track characteristic line of control reference point Ref-A-next excessively Tangent line is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool direction of advance；According to the left hand rule or the right hand of coordinate system Rule determines the positive direction of X-tool " axis and X-tool " axis；The position for controlling reference point Ref-A-next and posture information are made Target position and posture for tool in subsequent time, and asked according to the position and posture information of control reference point Ref-A-next Tool is in position between the target position of subsequent time of current target position and tool and attitude misalignment information out；Its Middle t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool, positioned at current time position Tool is moved to subsequent time target position after interval time t.At this point, actuator is according to control reference point in step (5) The position of Ref-A-next and posture information make the target position of tool move to subsequent time by current target position Target position.

Further, the 4th kind of Trajectory Tracking Control method above-mentioned, wherein position and attitude misalignment letter in step (4) The specific solution procedure of breath are as follows: set the tool coordinates system of the target position P point at tool current time as P-X-tool ' Y-tool ' Z-tool ',

△ z is: projected length of the vector of P point to control reference point Ref-A-next in Z-tool ' axis direction；

δ x is: by the track characteristic line linearization(-sation) being located at control reference point Ref-A-next or crossing control reference point Ref-A- Next makees the tangent line of track characteristic line, and the tangent line of track characteristic line linearization(-sation) or track characteristic line is crossing Y-tool ' axis and Z- The angle of Projection Line Segment S-yz and Y-tool ' axis on the cutting plane of tool ' axis；

δ y is: by the track characteristic line linearization(-sation) being located at control reference point Ref-A-next or crossing control reference point Ref-A- Next makees the tangent line of track characteristic line, and the tangent line of track characteristic line linearization(-sation) or track characteristic line is crossing X-tool ' axis and Z- The angle of Projection Line Segment S-xz and Z-tool ' axis on the cutting plane of tool ' axis；

δ z is: by the track characteristic line linearization(-sation) being located at control reference point Ref-A-next or crossing control reference point Ref-A- Next makees the tangent line of track characteristic line, and the tangent line of track characteristic line linearization(-sation) or track characteristic line is crossing X-tool ' axis and Y- The angle of Projection Line Segment S-xy and Y-tool ' axis in the plane of tool ' axis.

Further, the 4th kind of Trajectory Tracking Control method above-mentioned, wherein the amendment activation bit in step (5) is The synthesis activation bit that activation bit D-tool is combined at least one of scheduled task trace information, intervention information.

Further, the 4th kind of Trajectory Tracking Control method above-mentioned, wherein will be with sensor coordinate system in step (3) The position and attitude information of expression is converted into the conversion process of the position and attitude information of the expression of coordinate system accompanying by track are as follows: first It converts the position and attitude information expressed with sensor coordinate system to the position and attitude information expressed with tool coordinates system, then will The position and attitude information expressed with coordinate system accompanying by track is converted into the position and attitude information that tool coordinates system is expressed.

It realizes the Trajectory Tracking System of the 4th kind of Trajectory Tracking Control method, including sensor, tool and actuator, senses Device can perceive the location information and posture of track, and tool is attached on actuator, sensor be attached on the joint of actuator or Sensor is attached directly on tool, and the relative position between sensor and tool has determining geometrical relationship；Further include Infomation detection processor, Information application processor, positive kinematics module and inverse kinematics module are also wrapped in Information application processor Include information storage area；

Sensor acquires the initial trace information on workpiece track；

Information application processor calls the target position at calculated tool current time and posture information in positive kinematics module, And according to the target position at tool current time and posture information and track position posture information set { Q, N }, tool is determined Target position and posture in subsequent time, and find out tool current time position and tool in the target position of subsequent time Between position and attitude misalignment information；

Calculated tool is in current target position and tool under in inverse kinematics module recalls information application processor Position and attitude deviation information between the target position at one moment, and according to tool in current target position and tool under Position and attitude deviation information between the target position at one moment, the target position for calculating tool are located at subsequent time target position The joint angle information of actuator when setting.

It realizes the Trajectory Tracking System of the first Trajectory Tracking Control method, realize second of Trajectory Tracking Control method The 4th kind of Trajectory Tracking System, the Trajectory Tracking System for realizing the third Trajectory Tracking Control method and realization Trajectory Tracking Control The Trajectory Tracking System of method can be further divided into sensor-based system and execute system two parts, and specific division is as follows:

The first division mode: sensor and infomation detection processor constitute sensor-based system, and first is provided in sensor-based system Correspondence with foreign country device and first information interface end；Information application processor, positive kinematics module, inverse kinematics module and actuator structure At the system of execution, it is provided with the second correspondence with foreign country device and the second information interface end in execution system, sensor-based system and executes system System is electrically connected by first information interface end and the second information interface end.In actual use, sensor and infomation detection Processor can be made an entirety, and infomation detection processor can also be made into an individual backend information processor.Work Sensor-based system is passed down to execution system with the location information of the track of sensor coordinate system expression when making.

Second of division mode: sensor, infomation detection processor and Information application processor constitute sensor-based system, are passing The first correspondence with foreign country device and first information interface end are provided in sensing system；Positive kinematics module, inverse kinematics module and execution Device constitutes execution system, is provided with the second correspondence with foreign country device and the second information interface end in execution system, sensor-based system and holds Row system is electrically connected by first information interface end and the second information interface end.In actual use, sensor, information inspection It surveys processor and Information application processor can be made an entirety, infomation detection processor and Information application can also be handled Device is made into an individual backend information processor.When work, sensor-based system receives the tool current time that execution system uploads Target position and posture information；Sensor-based system passes down target position and the posture information of tool subsequent time to execution system.

The third division mode: sensor, infomation detection processor, Information application processor, positive kinematics module composition Sensor-based system is provided with the first correspondence with foreign country device and first information interface end in sensor-based system；Inverse kinematics module and execution Device constitutes execution system, is provided with the second correspondence with foreign country device and the second information interface end in execution system, sensor-based system and holds Row system is electrically connected by first information interface end and the second information interface end.In actual use, sensor, information inspection Survey processor, Information application processor and positive kinematics module can be made an entirety, can also by infomation detection processor, Information application processor and positive kinematics module are made into an individual backend information processor.When work, sensor-based system is received The joint angle information of the target position actuator at current target position for the tool that execution system uploads；Sensor-based system to Execution system passes down target position and the posture information of tool subsequent time.

4th kind of division mode: sensor, infomation detection processor, Information application processor, positive kinematics module, inverse fortune It is dynamic to learn module composition sensor-based system, the first correspondence with foreign country device and first information interface end are provided in sensor-based system；Actuator Execution system is constituted, the second correspondence with foreign country device and the second information interface end, sensor-based system and execution are provided in execution system System is electrically connected by first information interface end and the second information interface end.In actual use, sensor, infomation detection Processor, Information application processor, positive kinematics module and inverse kinematics module can be made an entirety, can also be by information Measurement processor, Information application processor, positive kinematics module and inverse kinematics module are made at an individual backend information Manage device.When work, the target position that sensor-based system receives the tool that execution system uploads is executed at current target position The joint angle information of device；Sensor-based system to pass under execution system tool subsequent time reach target position and posture when, actuator Joint angle information.

The beneficial effects of the present invention are: above-mentioned four kinds of Trajectory Tracking Control methods and corresponding track following system System, overcomes the advanced bring error of test point, can be detached from teaching information independent operating, can be sufficiently sharp during track following Track following is carried out with the information that sensor detects, the position deviation of tool can be not only corrected, the appearance of tool can also be corrected State deviation, so that track following purpose be better achieved.This outer grip Trajectory Tracking System is further divided into sensor-based system and holds It is more convenient, flexible can to make the foundation of tracking system for row system two parts, and convenient for being manufactured by different specialized factories, It improves performance, reduce cost.

Detailed description of the invention

Fig. 1 is that the structure of sensor and tool is shown in Trajectory Tracking Control method of the present invention and Trajectory Tracking System It is intended to.

Fig. 2 is the first division mode information of sensor-based system and execution system in Trajectory Tracking System of the present invention Relationship block diagram.

Fig. 3 is second of division mode information of sensor-based system and execution system in Trajectory Tracking System of the present invention Relationship block diagram.

Fig. 4 is the third division mode information of sensor-based system and execution system in Trajectory Tracking System of the present invention Relationship block diagram.

Fig. 5 is the 4th kind of division mode information of sensor-based system and execution system in Trajectory Tracking System of the present invention Relationship block diagram.

Fig. 6 is the schematic diagram that reference point locations are determined in the embodiment of the present invention three.

Fig. 7 is another schematic diagram for determining reference point locations in the embodiment of the present invention three.

Fig. 8 is the schematic diagram of the pitch angle deviation δ x of reference point in the embodiment of the present invention three.

Fig. 9 is the schematic diagram of the direction of advance angular deviation δ y of reference point in the embodiment of the present invention three.

Figure 10 is activation bit in the embodiment of the present invention three, scheduled task trace information, intervenes the synthesis that information combines The integrated information relationship block diagram of activation bit driving execution system.

Figure 11 is the deviation schematic diagram that reference point is controlled in the embodiment of the present invention four.

Figure 12 is the expression schematic diagram of original trace information in the embodiment of the present invention five.

Figure 13 is the comprehensive representation schematic diagram of trace information in the embodiment of the present invention five.

Figure 14 is the first schematic diagram of sensor-based system scarce capacity.

Figure 15 is second of schematic diagram of sensor-based system scarce capacity.

Figure 16 is sensor and the tool not schematic diagram on the same joint of actuator.

Figure 17 is the first schematic diagram of insufficient DOF actuator.

Figure 18 is second of schematic diagram of insufficient DOF actuator.

Figure 19 is the third schematic diagram of insufficient DOF actuator.

Specific embodiment

With reference to the accompanying drawing and preferred embodiment further details of the technical solution described in the present invention.

Embodiment one

Trajectory Tracking Control method described in the present embodiment, tool 2 are attached on actuator, and sensor 3 is attached to actuator On joint or sensor 3 is attached directly on tool 2, and the relative position between sensor 3 and tool 2 has determining geometry Relationship, specifically includes the following steps:

Step 1: detection track: acquiring the initial trace information of test point on the track on workpiece 1 by sensor 3, relate to here And the visual information that can will test of sensor 3 be converted into initial trace information, current this kind of sensor is generally vision biography Sensor, the present invention are illustrated by taking typical structured light vision sensor as an example.

Step 2: extracting trace information: extracting the location information of test point, and from initial trace information with sensor Coordinate system expresses the location information.

For convenience of description, carried out so that robotic tracking's V-weld, sensor 3 are set to welding gun front end as an example here Illustrate, actuator is robot here, and tool 2 is welding gun, and track is weld seam 4, and track characteristic line is characteristics of weld seam line, specific to tie Structure is as shown in Figure 1.The application of Trajectory Tracking Control method of the present invention and corresponding Trajectory Tracking System is not limited to welding neck Domain, as long as the task that guiding tool 2 is moved along continuous locus can be applicable in, such as with particular geometric side Border is the material cutting of foundation, gluing, the tasks such as corner of polishing.As shown in Figure 1, the seam cross-section letter that sensor 3 detects Breath concentrates in plane M-sense, and plane M-sense is defined as detection sectional plane here, coordinate system O-XYZ is defined as sensing Device coordinate system.There is a point that can represent on position while welding, such as corner connection section of weld joint two sides on the section of weld seam 4 The intersection point etc. of V word lowest part in intersection point, V-weld.This is represented the point letter of position while welding by we in detection sectional plane Referred to as test point, label is point in Fig. 1, and in welding, sensor 3 is inswept above weld seam 4, obtains a series of detection Point: Q₁、Q₂、Q₃……Q_i.The point for representing welding torch position is defined as welding gun characteristic point P, welding gun characteristic point P is generally in welding gun Position on axis, except tip, the distance for leaving tip are equal to electrode extension plus arc length.Robot is sensing It under a series of guidance of test point information of device 3, is moved with welding gun, welds welding gun characteristic point P along actual weld seam 4 It connects, the position and attitude matrix of welding gun characteristic point P is indicated with Tt.Any weld seam can be abstracted in alignment section or curved section, The exactly track to be tracked welding gun characteristic point P in welding, is called characteristics of weld seam line here.

As shown in Figure 1, the determination of coordinate system of the Butt welding gun at current location is as follows: welding gun is advanced in the welding process Direction definition be Y-tool ' axis positive direction, other two reference axis can as needed actual processing process definition, for convenience Description, define other two reference axis here in usual mode: X-tool ' axis vertical Y-tool ' axis and welding gun axis are constituted Plane, Z-tool ' axis has been determined that according to X-tool ' axis, Y-tool ' axis, the positive direction of Z-tool ' axis be leave welding gun, It is directed toward the direction of workpiece 1, the positive direction of X-tool ' axis is finally determined according still further to the right-hand rule.In Y-tool ' axis positive direction, For sensor 3 in front of welding gun, the coordinate system of welding gun is exactly coordinate system P-X-tool ' Y-tool that tool 2 is located at current time ' Z-tool ', the position and attitude transformation relation matrix T-sensor between the coordinate system and tool coordinates system of sensor 3 is known , thus for specific location posture changing relational matrix T-sensor, which is not described herein again.It is held if sensor 3 is attached to On other joints of row device, only it is to be understood that this transformation relation T-sensor, does not still influence subsequent calculating.

Step 3: label trace information: converting the location information expressed with sensor coordinate system to accompanying by track Coordinate system expression location information, be then marked in coordinate system accompanying by track, and be stored in shape in information storage area At track position information set { Q }；Coordinate system accompanying by track is workpiece coordinate system, workpiece when wherein the location of workpiece is not fixed Coordinate system accompanying by track is one of workpiece coordinate system or world coordinate system when position is fixed.

Robot works in world coordinate system, and in world coordinate system, the position of welding gun characteristic point P and posture can use it Position and attitude matrix Tt is indicated, is comprehensive description of Butt welding gun 2.Other than expressing in the matrix form, it is clear that position and attitude It can be stated with other forms, such as with the expression of the forms such as Eulerian angles or quaternary prime number.If workpiece 1 is not attached to generation In boundary's coordinate system, and it is attached in other coordinate systems, then track should also be stated with corresponding workpiece coordinate system, Yi Tong The realization of robotics knowledge is crossed, is repeated no more.

The position letter with the expression of coordinate system accompanying by track is converted by the location information expressed with sensor coordinate system The conversion process of breath are as follows:

The first, the location information expressed with sensor coordinate system is converted to the location information expressed with tool coordinates system, used Following formula: Q-t=(T-sensor) X(Q-s), wherein vector Q-t indicates description of the test point Q point in tool coordinates system, Q-s Indicate description of the test point Q point under sensor coordinate system, T-sensor is between sensor coordinate system and tool coordinates system Position and attitude transformation relation matrix；

The second, it converts the location information expressed with tool coordinates system to and is believed with the position of the expression of coordinate system accompanying by track Breath, using following formula: Q-w=(Tt) X Q-t=(Tt) X(T-sensor) X(Q-s), wherein vector Q-w indicates test point Q Description of the point in world coordinate system or workpiece coordinate system, the position and attitude matrix of Tt expression welding gun characteristic point P point.

Obviously, in aforementioned four formula, the coordinate of point should be homogeneous coordinates, rather than three-dimensional coordinate, only for succinctly not Separately use other symbolic formulations.As welding gun 2 advances with sensor 3, just obtain complete with a series of Q [i] point position The weld seam 4 of site preparation description, the line that this series of Q [i] point is sequentially connected, so that it may represent the actually detected characteristics of weld seam arrived Line is added to description information of this group to current Q point in track position information memory block, becomes a set, be denoted as { Q }, It is spare for subsequent step.

Step 4: determining target: being determined tool in the target position of subsequent time according to track position information set { Q } And posture, it is specific as follows: to set tool 2 in the target position at current time as P point, the tool coordinates system of P point is P-X-tool ' Y-tool ' Z-tool ' crosses P point and does the plane vertical with track characteristic line, and the intersection point of the plane and track characteristic line is reference Point Ref-A is tracking target with reference point Ref-A, using the position of reference point Ref-A as tool in the target position of subsequent time It sets.

Step 5: driving instrument: actuator according to the location information of reference point Ref-A, make the target position of tool 2 by working as Preceding moment target position moves to Ref-A point, and moves forward one along the positive direction of Y-tool ' axis using Ref-A as starting point Step pitch △=V*t, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool.

Step 6: circulation above steps, until completing the tracking of whole track.

Wherein, above-mentioned 4th step determines tool 2 in the target position of subsequent time according to track position information set { Q } And posture can be specifically unfolded, and the first is specifically unfolded as follows: tool 2 is set in the target position at current time as P point, P point Tool coordinates system be P-X-tool ' Y-tool ' Z-tool ', cross P point do the plane vertical with track characteristic line, the plane with The intersection point of track characteristic line is reference point Ref-A, is tracking target with reference point Ref-A, determines and be located at reference point Ref-A Tool coordinates system P-X-toolY-toolZ-tool: crossing the tangent line that reference point Ref-A makees track characteristic line, to cross track characteristic line Tangent line and to cross the plane of the straight line parallel with Z-tool ' axis be track divisional plane；Reference point Ref-A is crossed to do perpendicular to track The intersection of the plane of characteristic curve, the plane and track divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface； With the tangent line of the track characteristic line of Ref-A point excessively for Y-tool axis, the positive direction of Y-tool axis is directed toward tool direction of advance；According to The left hand rule or the right-hand rule of coordinate system determine the positive direction of X-tool axis and X-tool axis；By the position of reference point Ref-A And target position and posture of the posture as tool in subsequent time.At this time in the 5th step actuator according to reference point Ref-A's Position and posture information make the target position of tool 2 move to Ref-A point by the target position at current time, and are with Ref-A Starting point moves forward a step pitch △=V*t along the positive direction of Y-tool axis.

Since detection does not have posture information N in the first above-mentioned specific expansion, there cannot be track divisional plane, just not Posture can be adjusted, mends out the N lacked using the posture information of current tool Z-tool ' axis in the first specific expansion, thus Just there is track divisional plane, posture can be adjusted.

Ideally, the position and attitude relationship between welding gun axis and weld seam 4 is determined by welding procedure, then welding When connecing the weld seam 4, the axis of welding gun is exactly determining.Investigate a small weld seam section of point Q after testing, so that it may which this is micro- Small weld seam section is approximatively replaced with by a bit of straight line of Q point, then defining: after testing point Q point ideally The plane M-seam of welding gun axis and the straight line by representing weld seam is welding seam segmentation face.When by taking welding as an example, the first tool The track divisional plane referred in body expansion is exactly welding seam segmentation face.

Thus in addition to being mended out using the posture information in the first above-mentioned expansion mode using current tool Z-tool ' axis The mode of the N of missing obtains outside the divisional plane of track, can also be with Z-tool ' axis direction identical using one in second step Posture information of the vector as test point, and the posture information is expressed with sensor coordinate system.It then will be to pass by third step The posture information of sensor coordinate system expression is converted into the posture information expressed with coordinate system accompanying by track, is then marked on rail In coordinate system accompanying by mark, and it is stored in formation track posture information set { N }, track posture information collection in information storage area It closes and collectively forms track position posture information set { Q, N } with track position information set.

At this point, determining the work being located at reference point Ref-A according to track position posture information set { Q, N } in the 4th step Tool coordinate system P-X-toolY-toolZ-tool: it crosses reference point Ref-A and does the plane perpendicular to track characteristic line, the plane and rail The intersection of mark divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface；To cross the track characteristic line of Ref-A point Tangent line be Y-tool axis, the positive direction of Y-tool axis is directed toward 2 direction of advance of tool；According to the left hand rule or the right hand of coordinate system Rule determines the positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture information as tool under The target position at one moment and posture.At this time the 5th step actuator according to the position of reference point Ref-A and posture information, make tool 2 target position moves to Ref-A point by current target position, and is starting point along the pros of Y-tool axis using Ref-A To one step pitch △=V*t of forward movement.

For convenience of discussion, here by sensor 3 generate new information beat it is identical as the control beat of actuator for into Row illustrates that the beat that sensor 3 generates new information in actual use is likely to slower than the control beat of actuator.Because determining The information of amendment track does not need newest test point information, so only needing direct root when not new monitoring information generates The target of subsequent time is determined according to track position information set { Q }.

In the analysis of above each step by taking welding gun axis is perpendicular to track as an example, it is only for succinct.If practical Welding process requirement is worked with the angle of some non-90 degree, is not difficult tool to be adjusted to the angle by matrixing, to make It is applicable in the Trajectory Tracking Control method.

In above description, without reference to the period of the starting point that initially seeks trajectory of tracking.At this moment there are no accumulate any rail Mark information is needed in advance to be directed near the starting point of actual path tool 2 by other measures, then be initiated along Y-tool Direction advance acquisition track starting point movement, it is ensured that sensor can perceive the starting point of track in traveling, have accumulated to (a bit of trace information is obtained) after the trace information of few two test points, just really starts above-mentioned tracking process.

Above method is only to describe by taking the weld seam of V-butt as an example, and but not limited to this.For corner connection weld seam, overlap joint Weld seam, butt weld etc., as long as the weld bead feature points P defined in detection sectional plane can be applied.It is also complete for tasks such as gluings Above-mentioned Trajectory Tracking Control method can be referred to entirely.

Foregoing description is illustrated premised on 1 position of workpiece is fixed, when 1 position of workpiece is not fixed, can will be passed The coordinate system of sensor 3 and the coordinate system of tool 2 are all converted to be described in the form of workpiece coordinate system is reference data, at this time with regard to phase When the form fixed in the location of workpiece, thus fixed be not fixed with the location of workpiece of the location of workpiece can use rail described herein Mark tracking and controlling method.

The Trajectory Tracking System of realization Trajectory Tracking Control method, including sensor 3, tool 2 and actuator, tool 2 are attached In on actuator, it is finally the target position pursuit path by tool 2 that actuator band power driven tools 2, which move,.Sensor 3 adheres to In on the joint of actuator or sensor 3 is attached directly on tool 2, and the relative position between sensor 3 and tool 2 has Determining geometrical relationship further includes infomation detection processor 1011, Information application processor 1012,2011 and of positive kinematics module Inverse kinematics module 2012 further includes information storage area in Information application processor 1012；

Sensor 3 acquires the initial trace information of track on workpiece 1；

Infomation detection processor 1011 according to the initial trace information extraction that sensor 3 acquires go out track location information and with Sensor coordinate system expresses the location information；

Positive kinematics module 2011 obtains the joint angle letter of target position actuator at current target position of tool 2 Breath, and target position and posture information of the tool 2 at current time are calculated according to the joint angle information of the actuator；

Information application processor 1012 converts the location information for the track expressed with sensor coordinate system to accompanying by track Coordinate system expression location information, be then marked in coordinate system accompanying by track, and be stored in shape in information storage area At track position information set { Q }；

Information application processor 1012 calls the target position at 2 current time of tool and posture information in positive kinematics module, and It is located at target position and posture information and the track position information set { Q } at current time according to tool 2, determines that tool 2 exists The target position of subsequent time and posture information；

Target position and posture information of the tool 2 in subsequent time in 2012 recalls information application processor of inverse kinematics module, And target position and posture information according to tool 2 in subsequent time, the target position for calculating tool 2 are located at subsequent time mesh The joint angle information of actuator when cursor position.

Trajectory Tracking System can be further divided into 6 two parts of sensor-based system 5 and execution system, by sensor-based system 5 with hold The definite functions of row system 6 separate, and the two only needs frequent exchange information that can realize track following well.For convenience of description, Here sensor-based system 5 is sent into information to execution system 6 and is defined as " passing down ", system 6 will be executed to sensor-based system 5 and send information It is defined as " uploading ", the specific division of sensor-based system 5 and execution system 6 is as follows in Trajectory Tracking System:

As shown in Fig. 2, the first division mode is divided with the operating mode that sensor-based system provides information: sensor 3 and information are examined It surveys processor 1011 and constitutes sensor-based system 5, the first correspondence with foreign country device and first information interface end are provided in sensor-based system 5； Information application processor 1012, positive kinematics module 2011, inverse kinematics module 2012 and actuator constitute execution system 6, In The second correspondence with foreign country device and the second information interface end are provided in execution system 6, sensor-based system 5 and execution are 6 systems by first Information interface end and the electrical connection of the second information interface end.

When work, sensor-based system 5 does not need upload information, i.e. execution system 5 is not required to upload any letter to sensor-based system 5 Breath, it is only necessary to which sensor-based system 5 is passed down to execution system 6 with the location information of the track of sensor coordinate system expression.Sensing system System 5 mainly undertakes track position information acquisition, extraction task, and execution system 6 undertakes subsequent information processing work.When work, Sensor-based system 5 is expressed by the first correspondence with foreign country device, first information interface end to system 6 transmission of execution with sensor coordinate system Track position information, execute system 6 by the second information interface end, the second correspondence with foreign country device receive sensor-based system 5 transmit with The track position information of sensor coordinate system expression is simultaneously handled, and the target position that tool is located at subsequent time is obtained after processing And posture information, actuator move to the target position of tool 2 by current goal position according to above- mentioned information driving instrument 2 Next target position.

This kind is to the advantages of division mode of Trajectory Tracking System: 5 structure of sensor-based system is simple, can further reduce The volume of sensor-based system.

As shown in figure 3, second of division mode is divided with the drive mode that sensor-based system directly drives execution system: sensing Device 3, infomation detection processor 1011 and Information application processor 1012 constitute sensor-based system 5, and the is provided in sensor-based system 5 One external communicator and first information interface end；Positive kinematics module 2011, inverse kinematics module 2012 and actuator composition are held Row system 6 is provided with the second correspondence with foreign country device and the second information interface end, sensor-based system 5 and execution system in execution system 6 6 are electrically connected by first information interface end and the second information interface end.

When work, system 6 is executed by the second correspondence with foreign country device, the second information interface end to 5 means of transportation of sensor-based system The position at tool current time and posture information, sensor-based system 5 are held by first information interface end, the reception of the first correspondence with foreign country device Target position and the posture information at the tool current time that row system 6 transmits simultaneously are handled, and are obtained tool and are located at subsequent time Target position and posture information, then sensor-based system 5 by tool be located at subsequent time target position and posture information feed back To system 6 is executed, actuator driven tool 2 makes the target position of tool 2 move to subsequent time by current target position Target position.

As shown in figure 4, the third division mode is divided with the drive mode of sensor-based system and execution system agreement model: passing Sensor 3, infomation detection processor 1011, Information application processor 1012, positive kinematics module 2011 constitute sensor-based system 5, In The first correspondence with foreign country device and first information interface end are provided in sensor-based system 5；Inverse kinematics module 2012 and actuator are constituted Execution system 6, is provided with the second correspondence with foreign country device and the second information interface end in execution system 6, sensor-based system 5 and executes system System 6 is electrically connected by first information interface end and the second information interface end.

When work, system 6 is executed by the second correspondence with foreign country device, the second information interface end to 5 means of transportation 2 of sensor-based system Target position actuator at current target position joint angle information, sensor-based system 5 by the first correspondence with foreign country device, The target position for the tool 2 that first information interface end reception execution system 6 uploads actuator at current target position Joint angle information, since the information of upload itself is not enough to target position and the posture that direct solution tool 2 is located at subsequent time, The kinematics model for thus needing agreement actuator in advance, sensor-based system 5, execution system 6 is arranged the ginseng of mutually coordinated cooperation Number.Under sensor-based system 5 is located at according to the joint angle information of the actuator of upload and the kinematics model solution tool 2 of actuator The target position at one moment and posture information, and execution system 6 is fed back to, actuator driven tool 2 makes the target position of tool 2 It sets and subsequent time target position is moved to by current target position.

As shown in figure 5, the 4th kind of division mode is with sensor-based system and executes system agreement model and sensor-based system with joint The drive mode that angle controls execution system divides: sensor 3, infomation detection processor 1011, Information application processor 1012, just Kinematics module 2011, inverse kinematics module 2012 constitute sensor-based system 5, and the first correspondence with foreign country is provided in sensor-based system 5 Device and first information interface end；Actuator constitutes execution system 6, and the second correspondence with foreign country device and the are provided in execution system 6 Two information interface ends, sensor-based system 5 and execution system 6 are electrically connected by first information interface end and the second information interface end.

When work, system 6 is executed by the second correspondence with foreign country device, the second information interface end to 5 means of transportation 2 of sensor-based system Target position actuator at current target position joint angle information, sensor-based system 5 by the first correspondence with foreign country device, The target position for the tool 2 that first information interface end reception execution system 6 uploads actuator at current target position Joint angle information, since the information of upload itself is not enough to actuator when direct solution tool 2 is located at subsequent time target position Joint angle information, thus need in advance agreement actuator kinematics model, sensor-based system 5, execution system 6 are arranged mutually The parameter of cooperation.Sensor-based system 5 is solved according to the kinematics model of the joint angle information and actuator that upload actuator The joint angle information of actuator when tool is located at subsequent time target position, and execution system 6 is fed back to, actuator driven tool 2, so that the target position of tool 2 is moved to subsequent time target position by current target position.

It requires to receive execution system when second of division mode, the third division mode and the 4th kind of division mode work The kinestate information of the synchronization of upload is spare.The acquisition opportunity of the kinestate information acquires closer to sensor Moment is better, its advantage is that upload information can be utilized more accurately.

First information interface end involved in above-mentioned four kinds of division modes and the second information interface end can be made into information The form of interface nose, information interface female end, when first information interface end is nose, the second information interface end is then female end； When first information interface end is female end, the second information interface end is then nose.It in actual use can be by the first information Information interface is collectively referred to as after interface end and the electrical connection of the second information interface end.

Embodiment two

The present embodiment and embodiment one the difference is that: the present embodiment is on the basis of reference point Ref-A, not with above-mentioned Current control reference point Ref-A is tracking target, and is changed to control the control reference point Ref-A-next after interval time t To track target.Determined tool 2 in the target position of subsequent time in the present embodiment according to track position information set { Q } Specific method of determination is as follows: using reference point Ref-A as the centre of sphere, doing spherical surface by radius of △=V*t, the spherical surface and track characteristic line There are two intersection point, fetch bit is control reference point Ref-A- in the intersection point of spherical surface and track characteristic line in tool direction of advance Next determines that the tool being located at control reference point Ref-A-next is sat to control reference point Ref-A-next as tracking target Mark system P-X-tool " Y-tool " Z-tool ": crossing control reference point Ref-A-next and do the plane perpendicular to track characteristic line, should The intersection of plane and track divisional plane is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；Reference is controlled to cross The tangent line of the track characteristic line of point Ref-A-next is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool direction of advance； The positive direction of X-tool " axis and X-tool " axis is determined according to the left hand rule of coordinate system or the right-hand rule；Reference point will be controlled Target position and posture of the position and posture information of Ref-A-next as tool in subsequent time.

Position and posture information, the target that makes tool 2 of the actuator according to control reference point Ref-A-next in 5th step Position moves to subsequent time target position by current target position.

The specific expansion of other steps, each step of Trajectory Tracking Control method and the rail for realizing Trajectory Tracking Control method Mark tracking system is the same as example 1, and is repeated no more.

It is not tracking target with above-mentioned current control reference point Ref-A, and is changed to control the control after interval time t Reference point Ref-A-next is tracking target, and advantage of this is that use next control time adjustment of prediction that will will appear Deviation controlled, tracking performance is more preferable.Theoretically, all position and attitudes reached by control guidance are all ideal, no Deviation can be generated, that is, P [i+1] point is overlapped with Ref-A-next point, described " deviation " actually refers to current time state With the gap of subsequent time state.At this moment real error is only that the error generated in trace information detection, and executes system System controls the servo control error of generation not in place.

Embodiment three

The present embodiment and embodiment one the difference is that: the present embodiment determines in target in the 4th step, according to track position Information aggregate { Q } is set to determine that tool 2 on the basis of the target position of subsequent time and posture, also further finds out tool 2 In position between the target position of subsequent time of current target position and tool and attitude misalignment information, about the rail The specific expansion of other steps of mark tracking and controlling method is the same as example 1, and which is not described herein again.Specific track following control Method processed the following steps are included:

Step 1: detection track: acquiring the initial trace information of the track detection point on workpiece by sensor 3.

Step 4: determining target: being determined tool 2 in the target position of subsequent time according to track position information set { Q } Set and posture, and find out tool 2 in position between the target position of subsequent time of current target position and tool 2 and Attitude misalignment information, the position and attitude misalignment information be lateral deviation △ x or lateral deviation △ x and progressive deviation △ y, At least one of height tolerance △ z, pitch angle deviation δ x, direction of advance angular deviation δ z are combined.

Wherein determined tool 2 in the target position of subsequent time and the tool of posture according to track position information set { Q } Body method of determination is as follows: setting tool in the target position at current time as P point, the tool coordinates system of P point is P-X-tool ' Y- Tool ' Z-tool ' crosses P point and does the plane vertical with track characteristic line, and the intersection point of the plane and track characteristic line is reference point Ref-A is tracking target with reference point Ref-A, determines tool coordinates system P-X- that tool is located at reference point Ref-A ToolY-toolZ-tool: the tangent line that reference point Ref-A excessively makees track characteristic line, with the tangent line of track characteristic line and mistake and Z excessively The plane of the parallel straight line of-tool ' axis is track divisional plane；It crosses reference point Ref-A and does plane perpendicular to track characteristic line, it should The intersection of plane and track divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface；To cross the rail of Ref-A point The tangent line of mark characteristic curve is Y-tool axis, and the positive direction of Y-tool axis is directed toward tool direction of advance；It is fixed according to the left hand of coordinate system Then or the right-hand rule determines the positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture information as Target position and posture of the tool in subsequent time.

The wherein specific solution procedure of position and attitude misalignment information are as follows: set the work of tool current target position P point Tool coordinate system is P-X-tool ' Y-tool ' Z-tool ',

Other than above-mentioned position deviation, there is likely to be attitude angle deviations for welding gun, using tool coordinates system as referential, The several parameters for realizing robot trajectory's control are described.As shown in fig. 6, crossing the plane of X-tool ' axis, Y-tool ' axis Figure, middle conductor S-xy are the projection of characteristics of weld seam line on this plane.Since what is investigated is very little near welding gun characteristic point Size range, can be approximately straightway characteristics of weld seam line, so S-xy can also be regarded as straightway.It so needs to weld Rifle is δ z around the angular deviation of Z-tool ' axial adjustment, it is S-xy and Y-tool ' angle of axis.Determine other departures It needs to select a control reference point on characteristics of weld seam line, such point can choose the position Ref-A in Fig. 6.Due to reality Angle δ z very little in the work of border was P and crossed X-tool so control reference point also can choose the position Ref-B in Fig. 7 Reference point in the plane of axis does so to calculate to deviation and brings large error.Similarly, other departures are being calculated When, due to the angle very little of characteristics of weld seam line and each reference axis, so control reference point can select always it is above-mentioned Ref-A point also can choose Ref-B point, not will cause big error.That is: δ z is: being located at reference point on track characteristic line Projection S-xy and Y-tool ' of the tangent line in the plane for crossing X-tool ' axis and Y-tool ' axis at Ref-A or reference point Ref-B The angle of axis direction.

As shown in figure 8, crossing the cutting plane figure of Y-tool ' axis, Z-tool ' reference axis, middle conductor S-yz is characteristics of weld seam Projection of the line on the cutting plane.So need welding gun around X-tool ' axial adjustment angular deviation be δ x, i.e. S-yz and Y- The angle of tool ' axis.That is δ x is: being located at the tangent line at reference point Ref-A or reference point Ref-B on track characteristic line and is crossing Y- The angle of projection S-yz and Y-tool ' axis direction in the plane of tool ' axis and Z-tool ' axis.

As shown in figure 9, crossing the cutting plane figure of X-tool ' axis, Z-tool ' axis, middle conductor S-xz is that characteristics of weld seam line exists Projection on the cutting plane, then need welding gun around Y-tool ' axial adjustment angular deviation be δ y, i.e. S-xz and Z-tool ' The angle of axis.Fig. 9 goes back while expressing position adjustment amount △ x, the △ z of welding gun when using Ref-B as reference point.

Above-mentioned departure can be synthetically expressed as a six-vector, be denoted as:

D-tool=[△ x, △ y, △ z, δ x, δ y, δ z], wherein y=0 △.

This is namely the statement under tool coordinates system to the complete description of tool path deviation.Since it can be straight It connects for driving actuator, alternatively referred to as activation bit.

Because of here, y=0 △, it is clear that the activation bit on Y-tool change in coordinate axis direction will be supplemented out by executing internal system Tracking can be just pushed ahead, machine man-hour is just moved with welding speed V along Y-tool change in coordinate axis direction.At this moment, so that it may To think that tracking is the synthesis result of the drive command D-tool outside execution and internal propulsion drive command.

The velocity information V of the tool pursuit path is that current time is received from execution system or be sensor-based system It is voluntarily determining or be to execute default or arranged between execution system and sensor-based system, do not influence this The core concept of invention.In special circumstances, execution system ignores the speed adjustment information △ y of itself, and at this moment V=0, can also bring The effect of special requirement: if again in Y-tool ' axis direction, correction amount of the outside to robot movement velocity be changed to △ y= V*t, wherein t is the adjustment interval time of each Correction and Control, then robot only leans on external D-tool driving that can realize Tracking.At this moment robot is exactly one completely by the equipment of external sensor-based system driving.If generated with other means another A each component meaning equivalent six-vector D-tool-2 corresponding with D-tool then can be used for driving execution system, such as scheme Described in 10, it might even be possible to drive execution system with the combining information of various activation bits.

Step 5: driving instrument: actuator is according to the position of reference point Ref-A and attitude misalignment information, the mesh for making tool 2 Cursor position moves to Ref-A point by the target position at current time, and moves forward one along predetermined direction using Ref-A as starting point A step pitch △=V*t, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool.Make work The target position of tool 2 is activation bit D-tool by the amendment activation bit that the target position at current time moves to Ref-A point, Activation bit D-tool is to contain at least one active constituent: the vector of △ x, and it includes five that activation bit D-tool, which is at most, Active constituent: the vector of △ x, △ y, △ z, δ x, δ z.It is described predetermined wherein when activation bit D-tool does not include δ x or δ z Direction is naturally determined direction due to adjustment of displacement, this refers to ignore the adjustment of posture, only by △ x, △ y, At least one position that generates between △ z adjusts, and thus change in location determines the direction of advance of tool；In activation bit D- When tool includes δ x or δ z, the predetermined direction is the positive direction of Y-tool axis.

It realizes in the track following scheme of fully automated control, so-called " fully automated control " refers to can be not against preparatory The track write describes program (such as tutorial program), and can track automatically according to track characteristic adjust automatically.At this moment in Figure 10 " teaching track " information do not work.That is: amendment activation bit can be only individual activation bit D-tool.

During not can confirm that tracking in the environment without interference and collision, it can also be evaded by simple teaching information Those foreseeable obstacles, what is at this moment utilized is " teaching track " information in Figure 10, by the information determine tool posture or A part of freedom degree in person's posture, such as δ y or δ y and δ z, it can be ensured that do not collide.Even such case, teaching letter Breath is also the complicated teaching information for determining travel track simplified very much, rather than conventional.That is amendment activation bit can also be with It is the comprehensive driving letter that activation bit D-tool is combined at least one of scheduled task trace information, intervention information Breath.

Thus the amendment activation bit in the 5th step can be only activation bit D-tool, be also possible to activation bit D- The synthesis activation bit that tool is combined at least one of scheduled task trace information, intervention information.

If it is rail that external sense system is defined using the tool coordinates system of actuator as referential that it is received, which to execute system, Mark update information, execution system can also receive manual control and be formed, is fixed by referential of the tool coordinates system of actuator The track correct information of justice, this information is in conjunction with the deviation control information that received external sense system provides, then with knot Information after conjunction carries out track correct；Deviation information in conjunction with method it is different according to task and determine.Symbol Σ is not generation in figure Each dominant vector is simply added by table, but representative information merges, and can have selection, can be enhanced or weaken control amount The variation such as ratio.For example, people uses a six-dimensional force/torque sensor, the letter of most 6 dimensions relative to operator is generated Breath: manpower motive force/torque sensor issues positive △ y information, and pulling issues negative △ y information, makes the size of △ y It is directly proportional to the dynamics of push-and-pull；The component addition corresponding with D-tool of this information or weighting summation, then can be real Existing intervention of the people to tracking velocity；Similarly, lateral push-pull effort/torque sensor can be used, realizing temporarily makes tool deviate tracking The adjustment of track；The torque that torque forces/torque sensor generates then can be used for the adjustment to tool posture.It can certainly " mistake The control information for being not intended to generate is fallen in filter ", to inhibit the information for being not intended to variation.In order to avoid on certain undesirable dimension It interferes, can be masked, such as ignore the lateral deviation adjustment information to tool accidentally generated.Here it is deviations The principle that information combines.It can be used for causing such human intervention information with the system for being similar to game paddle.

Step 6: circulation above steps, until completing the tracking of whole track.

The Trajectory Tracking System of Trajectory Tracking Control method, including sensor 3, tool and execution are realized in the present embodiment Device, tool 2 are attached on actuator, and sensor 3 is attached on the joint of actuator or sensor 3 is attached directly on tool 2, And the relative position between sensor 3 and tool has determining geometrical relationship, further includes infomation detection processor 1011, information Application processor 1012, positive kinematics module 2011 and inverse kinematics module 2012 further include in Information application processor 1012 Information storage area；

Sensor 3 acquires the initial trace information of track on workpiece 1；

Infomation detection processor 1011 according to the initial trace information extraction that sensor acquires go out track location information and with pass Sensor coordinate system expresses the location information；

Information application processor 1012 calls the position at tool current time and posture information in positive kinematics module, and according to work Have position and posture information and the track position information set { Q } at current time, determines tool in the target position of subsequent time Set and posture information, and find out tool current time position and position and posture of the tool between the target of subsequent time it is inclined Poor information；

In 2012 recalls information application processor of inverse kinematics module tool 2 in current target position with tool 2 next Position and attitude misalignment information between the target position at moment, and existed according to tool 2 in current target position and tool 2 Position and attitude misalignment information between the target position of subsequent time, the target position for calculating tool 2 are located at subsequent time Joint angle information when target position.

Trajectory Tracking System can be further divided into 6 two parts of sensor-based system 5 and execution system, by sensor-based system 5 with hold The definite functions of row system 6 disclose, and the two only needs frequent exchange information that can realize track following well.It is described for aspect, Here sensor-based system 5 is sent into information to execution system 6 and is defined as " passing down ", system 6 will be executed to sensor-based system 5 and send information It is defined as " uploading ", sensor-based system 5 and the specific division for executing system 6 and four kinds of divisions in embodiment one in Trajectory Tracking System Mode is identical, and specific working mode is also identical, and which is not described herein again.

Example IV

The present embodiment and embodiment three the difference is that: the present embodiment is on the basis of reference point Ref-A, not with above-mentioned Current control reference point Ref-A is tracking target, and is changed to control the control reference point Ref-A-next after interval time t To track target.Determined tool in the target position of subsequent time in the present embodiment according to track position information set { Q } Specific method of determination is as follows: using reference point Ref-A as the centre of sphere, doing spherical surface by radius of △=V*t, the spherical surface and track characteristic line There are two intersection point, fetch bit is control reference point Ref-A- in the intersection point of spherical surface and track characteristic line in tool direction of advance Next, to control reference point Ref-A-next as tracking target, the tool that determines is located at the work at control reference point Ref-A-next Tool coordinate system P-X-tool " Y-tool " Z-tool ": it crosses control reference point Ref-A-next and does perpendicular to the flat of track characteristic line The intersection of face, the plane and track divisional plane is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；It is controlled with crossing The tangent line of the track characteristic line of reference point Ref-A-next is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool advance side To；The positive direction of X-tool " axis and X-tool " axis is determined according to the left hand rule of coordinate system or the right-hand rule；Control is referred to Target position and posture of the position and posture information of point Ref-A-next as tool in subsequent time；And it is referred to according to control The position of point Ref-A-next and posture information find out tool current time position and tool subsequent time target position it Between position and attitude misalignment information, the position and attitude misalignment information be lateral deviation △ x or lateral deviation △ x with before It is combined at least one of deviation △ y, height tolerance △ z, pitch angle deviation δ x, direction of advance angular deviation δ z.

The wherein specific solution procedure of position and attitude misalignment information are as follows: the tool for setting current target position P point is sat Mark system is P-X-tool ' Y-tool ' Z-tool ',

Position and posture information, the mesh that makes tool of the actuator according to control reference point Ref-A-next in 5th step at this time Cursor position moves to subsequent time target position by current target position.

The specific expansion of other steps, each step of Trajectory Tracking Control method and the rail for realizing Trajectory Tracking Control method Mark tracking system is identical as embodiment three, repeats no more.

It is not tracking target with above-mentioned current control reference point Ref-A, and is changed to control the control after interval time t Reference point Ref-A-next is tracking target, then △ x, △ y, the meaning of δ z are as shown in figure 11.At this moment, other control amounts are calculated Reference point will also make corresponding modification (only must carry out above-mentioned calculating Ref-A-next as Ref-A point), specially use curve here The value and difference when being tracking target with Ref-A that track shows δ z.In Figure 11, △ be between Ref-A and Ref-A-next Distance on three-dimensional space.Advantage of this is that, using prediction next control time adjustment by the deviation that will appear into Row control, tracking performance are more preferable: theoretically, all position and attitudes reached by control guidance are all that preferably, will not generate partially Difference, that is, P [i+1] point is overlapped with Ref-A-next point, described " deviation " actually refers to current time state and lower a period of time Quarter state gap.At this moment real error is only that the error generated in trace information detection, and the system control of execution is not The servo control error generated in place.

Above-described embodiment one, embodiment two, embodiment three and trace tracking method in example IV and corresponding As long as sensor 3 involved in Trajectory Tracking System has deviation of the perception target trajectory on the direction for leaving tool, in-orbit The ability of deviation in two side directions of mark, even if workpiece where the sensor can not perceive track is surface in test point The characteristic for locating " towards which direction ", is called " posture " of track herein, can also realize track following well.

Following embodiments five, embodiment six, embodiment seven and trace tracking method in embodiment eight and corresponding Sensor involved in Trajectory Tracking System then need with can perceive deviation of the target trajectory on the direction for leaving tool, Deviation in two side directions of track, can also perceive workpiece where track be surface at test point " towards which direction " Characteristic is called the ability of " posture " of track herein.

Embodiment five

The present embodiment and embodiment one the difference is that: the sensor in the present embodiment can not only perceive target trajectory and exist The deviation on the direction of tool, the deviation in two side directions of track are left, is collectively referred to as the location information of track, moreover it is possible to perceive Workpiece where track is characteristic of the surface at test point " towards which direction ", i.e. posture information.About the track following control The specific expansion of each step of method processed is the same as example 1, and which is not described herein again.

Specific Trajectory Tracking Control method described in the present embodiment the following steps are included:

Step 1: detection track: acquiring the initial trace information of test point on track on workpiece 1 by sensor 3.

Step 2: extracting trace information: extracting the location information of test point from initial trace information and in the detection The posture information of the track Dian Chu divisional plane, and the location information and posture information are expressed with sensor coordinate system.

In welding, sensor 3 is inswept above weld seam 4, obtains a series of test point Q, robot is in sensor It under a series of guidance of test point information, is moved with welding gun, welds welding gun characteristic point P along actual weld seam, welded The position and attitude matrix of rifle characteristic point P is indicated with Tt.

Any weld seam can be abstracted in alignment section or curved section, exactly the track to be tracked in welding, referred to here as It is characteristics of weld seam line.So, so-called weld joint tracking is exactly so that welding gun characteristic point P is not departed from weld seam in welding gun motion process Characteristic curve, and the particular angular relationships for making welding gun axis and characteristics of weld seam line that welding procedure be kept to need.

Ideally, the position and attitude relationship between welding gun axis and weld seam is determined by welding procedure, then welding When connecing the weld seam, the axis of welding gun is exactly determining.Investigate a small weld seam section of point Q after testing, so that it may which this is micro- Small weld seam section is approximatively replaced with by a bit of straight line of Q point, then defining: after testing point Q point ideally The plane M-seam of welding gun axis and the straight line by representing weld seam is welding seam segmentation face.What sensor detected is to examine The profiled outline for surveying the weld seam in the M-sense of section, can calculate welding seam segmentation face M-seam according to the profile and detection is cut The intersection is expressed as vector N-seam by the intersection of face M-sense, it can represent the direction in welding seam segmentation face, referred to as weld seam Attitude vectors or abbreviation weld seam posture.

In fact, otherwise Q point excessively is available more unless detection sectional plane M-sense is vertical with welding seam segmentation face M-seam A weld seam attitude vectors N-seam.But this has no effect on the mission of N-seam: being exactly in order to by N-seam and a bit of rail Mark determined the welding seam segmentation face M-seam that Q point is determined jointly, so, as long as M-sense intersects with M-seam, obtained N- Seam can act as same effect.

Fig. 1 is that the reference axis of sensor coordinate system is parallel to weld seam and a kind of agonic special state, and Figure 12 is then General state is represent, the content as shown in the figure to be measured on detection sectional plane M-sense, it

It is the weld profile of a series of point or arc description, information is very abundant.We will extract most effective, few as far as possible letter Breath, the characteristic of Lai Daibiao weld seam.We select the location information of Q point and vector N-seam to represent the characteristic of weld seam jointly.As it can be seen that If indicating description of the Q point under sensor coordinate system, Q-s=[Xs, 0, Zs] with vector Q-s.

According to geometrical relationship, the vector of point a to point e are rotated by 90 ° counterclockwise, so that it may for expressing vector N- Seam, as [- dz/L, 0, dx/L].Indicate description of the N-seam under sensor coordinate system with vector N-s, then N-s= [- dz/L, 0, dx/L].

As it can be seen that in weld information Q-s, the N-s for needing to describe to the transmission of the system of execution with sensor coordinate system, it can be only 4 data are transmitted, because there are 20 in their coordinate expression, can acquiescently be obtained.It further, can also be N-s normalizing Unit vector is turned to, then, it can also reduce by 1 transmission data by normalization.Certainly, posture information N-s can also use 1 Angle δ y is expressed.

Step 3: label trace information: converting the position and posture information expressed with sensor coordinate system to track The position of accompanying coordinate system expression and posture information, are then marked in coordinate system accompanying by track, and be stored in letter It ceases in memory block and forms track position posture information set { Q, N }；Coordinate accompanying by track when wherein the location of workpiece is not fixed System is workpiece coordinate system, and coordinate system accompanying by track is one in workpiece coordinate system or world coordinate system when the location of workpiece is fixed Kind.

Robot works in world coordinate system, and in world coordinates, the position of welding gun characteristic point P and posture can use its position Attitude matrix Tt expression is set, is comprehensive description of Butt welding gun.Other than expressing in the matrix form, it is clear that position and attitude can also To be stated with other forms, such as with the expression of the forms such as Eulerian angles or quaternary prime number.The position that will be expressed with sensor coordinate system Set and posture information be converted into coordinate system accompanying by track expression position and posture information conversion process are as follows:

The first, it converts the position and posture information expressed with sensor coordinate system to the position of tool coordinates system expression and appearance State information, using following formula:

Q-t=(T-sensor) X(Q-s)

N-t=(T-sensor) X(N-s)

Wherein vector Q-t indicates description of the target position in tool coordinates system of subsequent time, and Q-s indicates the target of subsequent time Description of the position under sensor coordinate system, position and attitude of the T-sensor between sensor coordinate system and tool coordinates system become Change relational matrix；Vector N-s indicates description of the vector N-seam under sensor coordinate system, and vector N-t indicates vector N-seam Description under tool coordinates system；

The second, the position and posture information expressed with tool coordinates system are converted to the position expressed with coordinate system accompanying by track It sets and posture information, using 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 vector Q-w indicates description of the target position in world coordinate system or workpiece coordinate system of subsequent time, and Tt indicates work Tool is located at the position and attitude matrix of current time position P point, and vector N-w indicates that vector N-seam is sat in world coordinate system or workpiece Description under mark system.

Obviously, in aforementioned four formula, the coordinate of point should be homogeneous coordinates, rather than three-dimensional coordinate, only for succinctly not Separately use other symbolic formulations.Therefore, as shown in figure 13, advance with welding gun with sensor, just obtained with a series of The point position Q [i] and its subsidiary vector N-seam(be abbreviated as N [i]) weld seam that is fully described by, by this series of Q point The line being sequentially connected, so that it may represent the actually detected characteristics of weld seam line arrived；The curved surface being made of each N-seam can generation The actually detected welding seam segmentation face arrived of table.Description information of this group to current Q point is added to the storage of track position posture information Qu Zhong becomes a set, is denoted as { Q, N }, is that subsequent step is spare.

If workpiece is not attached in world coordinate system, and it is attached in other coordinate systems, then track also should It is stated with corresponding workpiece coordinate system, is easily realized, repeated no more by robotics knowledge.

As shown in Fig. 13 it the case where one distorting, is welded on non-expandable curved face, can represent to appointing The processing method of meaning space curve track.In order not to cover characteristics of weld seam line, by the direction of this vector N-seam a series of in figure It is inverted.The direction definition that welding gun advances in the welding process is the positive direction of the Y-tool axis of tool coordinates system.Theoretically in addition Two reference axis can be defined arbitrarily, in order to describe simplicity, do following hypothesis here in usual mode: X-tool axis perpendicular to The plane that Y-tool axis and welding gun axis are constituted, and the positive direction of Z-tool, for the direction left welding gun, be directed toward workpiece, finally The positive direction of X-tool is determined according still further to the right-hand rule.

Step 4: determining target: being determined tool in the mesh of subsequent time according to track position posture information set { Q, N } Cursor position, it is specific as follows: to set tool in the target position at current time as P point, cross P point and do vertical with track characteristic line put down The intersection point of face, the plane and track characteristic line is reference point Ref-A, is tracking target with reference point Ref-A, determines and be located at reference Tool coordinates system P-X-toolY-toolZ-tool at point Ref-A: it crosses reference point Ref-A and does perpendicular to track characteristic line The intersection of plane, the plane and track divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface；It is referred to crossing The tangent line of the track characteristic line of point Ref-A is Y-tool axis, and the positive direction of Y-tool axis is directed toward tool direction of advance；According to seat The left hand rule or the right-hand rule of marking system determine the positive direction of X-tool axis and X-tool axis；Sensor is detected in Ref-A point Be trajectory line in the detection sectional plane M-sense of sensor profiled outline, track point is calculated according to the profiled outline The intersection is expressed as vector N-seam by the intersection of face M-seam and detection sectional plane M-sense, and vector N-seam is tool position In the posture of reference point Ref-A, using the position of reference point Ref-A and posture information as tool in the target position of subsequent time And posture.

Step 5: driving instrument: actuator is according to the position of reference point Ref-A and posture information, the target position for making tool Set and Ref-A point moved to by the target position at current time, and using Ref-A as starting point along the positive direction of Y-tool axis to Forward A step pitch △=V*t is moved, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool.

Step 6: circulation above steps, until completing the tracking of whole track.

The Trajectory Tracking System of Trajectory Tracking Control method, including sensor 3, tool 2 and execution are realized in the present embodiment Device, tool are attached on actuator, and sensor 3 is attached on the joint of actuator or sensor 3 is attached directly on tool, and Relative position between sensor 3 and tool 2 has determining geometrical relationship, further includes infomation detection processor 1011, information Application processor 1012, positive kinematics module 2011 and inverse kinematics module 2012 further include in Information application processor 1012 Information storage area；

Sensor 3 acquires the initial trace information of track on workpiece；

Infomation detection processor 1011 goes out the position of track according to the initial trace information extraction that sensor 3 acquires and posture is believed It ceases and the position and posture information is expressed with sensor coordinate system；

Positive kinematics module 2011 obtains the joint angle information of target position actuator at current target position of tool, And target position and posture information of the tool 2 at current time are calculated according to the joint angle information of the actuator；

Information application processor 1012 converts the track position and posture information expressed with sensor coordinate system to track institute The track position and posture information of the coordinate system expression of attachment, are then marked in coordinate system accompanying by track, and be stored in Track position posture information set { Q, N } is formed in information storage area；

Information application processor calls the target position at 2 current time of tool and posture information in positive kinematics module 2011, and According to tool 2 in the target position at current time and posture information and track position posture information set { Q, N }, work is determined Target position and posture of the tool 2 in subsequent time；

Tool 2 is believed in the target position of subsequent time and posture in 2012 recalls information application processor 1012 of inverse kinematics module Breath, and target position and posture according to tool 2 in subsequent time, the target position for calculating tool 2 are located at subsequent time mesh The joint angle information of actuator when cursor position.

Embodiment six

The present embodiment and embodiment five the difference is that: the present embodiment is on the basis of reference point Ref-A, not with above-mentioned Current control reference point Ref-A is tracking target, and is changed to control the control reference point Ref-A-next after interval time t To track target.Determined tool in the target of subsequent time in the present embodiment according to track position posture information set { Q, N } The specific method of determination of position is as follows: using reference point Ref-A as the centre of sphere, doing spherical surface by radius of △=V*t, the spherical surface and track Characteristic curve is control reference point Ref- in the intersection point of spherical surface and track characteristic line in tool direction of advance there are two intersection point, fetch bit A-next determines the tool being located at control reference point Ref-A-next to control reference point Ref-A-next as tracking target Coordinate system P-X-tool " Y-tool " Z-tool ": crossing control reference point Ref-A-next and do the plane perpendicular to track characteristic line, The intersection of the plane and track divisional plane is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；Ginseng is controlled to cross The tangent line of the track characteristic line of examination point Ref-A-next is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool advance side To；The positive direction of X-tool " axis and X-tool " axis is determined according to the left hand rule of coordinate system or the right-hand rule；Sensor exists What Ref-A-next point detected is the profiled outline of the trajectory line in the detection sectional plane M-sense of sensor, disconnected according to this Facial contour calculates the intersection of track divisional plane M-seam and detection sectional plane M-sense, which is expressed as vector N-seam, Vector N-seam be tool be located at control reference point Ref-A-next posture, by control reference point Ref-A-next position and Target position and posture of the posture information as tool in subsequent time；Wherein t is the adjustment of each position correction control of tool Interval time, V are the forward speed of tool, and the tool positioned at current time position is moved to subsequent time after interval time t Target position.

Position and posture information, the target position that makes tool of the actuator according to control reference point Ref-A-next in 5th step Target position correction motion by current time is set to the target position of subsequent time.

The specific expansion of other steps, each step of Trajectory Tracking Control method and the rail for realizing Trajectory Tracking Control method Mark tracking system is identical as embodiment five, repeats no more.

Embodiment seven

The present embodiment and embodiment five the difference is that: the present embodiment determines in target in the 4th step, according to track position Set information aggregate { Q, N } determine tool the target position of subsequent time and on the basis of, also further to find out tool and work as The position deviation information of preceding moment position and tool between the target position of subsequent time, about the Trajectory Tracking Control method The specific expansion of other steps is identical as embodiment five, and which is not described herein again.

The specific expansion of applicability and other steps about the Trajectory Tracking Control method is identical as embodiment five, this In repeat no more.

Specific Trajectory Tracking Control method in the present embodiment the following steps are included:

Step 1: detection track: acquiring the initial trace information of test point on the track on workpiece 1 by sensor 3；

Step 2: extracting trace information: from the location information for extracting test point in initial trace information and at the test point The posture information of track divisional plane, and the location information and posture information are expressed with sensor coordinate system；

Step 3: label trace information: converting the position and posture information expressed with sensor coordinate system to appended by track Coordinate system expression position and posture information, be then marked in coordinate system accompanying by track, and be stored in information and deposit Track position posture information set { Q, N } is formed in storage area；Coordinate system accompanying by track is when wherein the location of workpiece is not fixed Workpiece coordinate system, coordinate system accompanying by track is one of workpiece coordinate system or world coordinate system when the location of workpiece is fixed.

Convert the position and attitude information expressed with sensor coordinate system to the position expressed with coordinate system accompanying by track Set the conversion process of posture information are as follows:

The first, it converts the position and attitude information expressed with sensor coordinate system to and is believed with the position and attitude of tool coordinates system expression Breath, using following formula:

Q-t=(T-sensor) X(Q-s)

N-t=(T-sensor) X(N-s)

Wherein vector Q-t indicates description of the target position in tool coordinates system of subsequent time, and Q-s indicates the target of subsequent time Description of the position under sensor coordinate system, position and attitude of the T-sensor between sensor coordinate system and tool coordinates system become Change relational matrix；What sensor was detected in Ref-A point is the track characteristic line in the detection sectional plane M-sense of sensor Profiled outline can calculate the intersection of track divisional plane M-seam and detection sectional plane M-sense, by this according to the profiled outline Intersection is expressed as vector N-seam；Vector N-s indicates description of the vector N-seam under sensor coordinate system, and vector N-t is indicated Description of the vector N-seam under tool coordinates system；

The second, the position and attitude information expressed with tool coordinates system is converted to the position expressed with coordinate system accompanying by track Posture information, using 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)

Step 4: determining target: being determined tool in the mesh of subsequent time according to track position posture information set { Q, N } Cursor position and posture, and find out position of the tool in current target position and attitude and tool between the target of subsequent time And attitude misalignment information, the position and attitude misalignment information is lateral deviation △ x, progressive deviation △ y, height tolerance △ z, Pitch angle deviation δ x, lateral deviation angular displacement δ y, at least x containing △, δ y two in direction of advance angular deviation δ z this six active constituents The position deviation information of a active constituent, and it includes △ x, △ y, △ z, δ x, δ that the position deviation and posture information, which is at most, The position deviation information of six active constituents of y, δ z；Wherein tool is determined according to according to track position posture information set { Q, N } It is as follows in the target position of subsequent time and the specific method of determination of posture:

If tool is P point in the target position at current time, crosses P point and do the plane vertical with track characteristic line, the plane and rail The intersection point of mark characteristic curve is reference point Ref-A, is tracking target with reference point Ref-A, determines the work being located at reference point Ref-A Tool coordinate system P-X-toolY-toolZ-tool: crossing reference point Ref-A and do the plane perpendicular to track characteristic line, the plane with The intersection of track divisional plane is Z-tool axis, and the positive direction of Z-tool axis is directed toward workpiece surface；To cross the track of reference point Ref-A The tangent line of characteristic curve is Y-tool axis, and the positive direction of Y-tool axis is directed toward tool direction of advance；According to the left hand rule of coordinate system Or the right-hand rule determines the positive direction of X-tool axis and X-tool axis；What sensor was detected in Ref-A point is in sensor The profiled outline of trajectory line in detection sectional plane M-sense calculates track divisional plane M-seam and inspection according to the profiled outline The intersection is expressed as vector N-seam by the intersection for surveying section M-sense, and vector N-seam is that tool is located at reference point Ref-A Posture, target position and posture using the position of reference point Ref-A and posture information as tool in subsequent time.

The wherein specific solution procedure of position and attitude misalignment information are as follows: be set in the work of current target position P point Tool coordinate system is P-X-tool ' Y-tool ' Z-tool ',

Step 5: driving instrument: actuator makes the mesh of tool according to the position reference point Ref-Ade and attitude misalignment information Cursor position is moved to the target position of subsequent time by current target position.Make the target position of tool by current time mesh The amendment activation bit that cursor position moves to the target position of subsequent time is activation bit D-tool, and activation bit D-tool is Including at least two active constituents: the vector of △ x, δ y, and it includes six active constituents: △ x that activation bit D-tool, which is at most, The vector of △ y, △ z, δ x, δ y, δ z.

Step 6: circulation above steps, until completing the tracking of whole track.

Realize the Trajectory Tracking System of Trajectory Tracking Control method, including sensor 3, tool and actuator, tool attachment In on actuator, sensor 3 is attached on the joint of actuator or sensor 3 is attached directly on tool, and sensor 3 and work Relative position between tool has determining geometrical relationship, further includes infomation detection processor 1011, Information application processor 1012, positive kinematics module 2011 and inverse kinematics module 2012 further include information storage area in Information application processor 1012；

Sensor 3 acquires the initial trace information of track on workpiece 1；

Information application processor 1012 call positive kinematics module 2011 in calculated 2 current time of tool target position and Posture information, and according to the target position at 2 current time of tool and posture information and track position posture information set Q, N }, determine target position and posture information of the tool 2 in subsequent time, and find out tool 2 in current target position and work Position and attitude misalignment information of the tool 2 between the target position of subsequent time；

Inverse kinematics module 2012 is capable of calling in Information application processor 1012 tool 2 in current target position and tool 2 Position and attitude deviation information between the target position of subsequent time, and according to tool 2 in current target position and work Position and attitude deviation information of the tool 2 between the target position of subsequent time, the target position for calculating tool is located at lower a period of time The joint angle information of actuator when carving target position.

Embodiment eight

The present embodiment and embodiment seven the difference is that: the present embodiment is on the basis of reference point Ref-A, not with above-mentioned Current control reference point Ref-A is tracking target, and is changed to control the control reference point Ref-A-next after interval time t To track target.Determined tool in the target of subsequent time in the present embodiment according to track position posture information set { Q, N } The specific method of determination of position is as follows: using reference point Ref-A as the centre of sphere, doing spherical surface by radius of △=V*t, the spherical surface and track Characteristic curve is control reference point Ref- in the intersection point of spherical surface and track characteristic line in tool direction of advance there are two intersection point, fetch bit A-next determines the tool being located at control reference point Ref-A-next to control reference point Ref-A-next as tracking target Coordinate system P-X-tool " Y-tool " Z-tool ": crossing control reference point Ref-A-next and do the plane perpendicular to track characteristic line, The intersection of the plane and track divisional plane is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；Ginseng is controlled to cross The tangent line of the track characteristic line of examination point Ref-A-next is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool advance side To；The positive direction of X-tool " axis and X-tool " axis is determined according to the left hand rule of coordinate system or the right-hand rule；Sensor exists What Ref-A-next point detected is the profiled outline of the trajectory line in the detection sectional plane M-sense of sensor, disconnected according to this Facial contour calculates the intersection of track divisional plane M-seam and detection sectional plane M-sense, which is expressed as vector N-seam, Vector N-seam be tool be located at control reference point Ref-A-next posture, by control reference point Ref-A-next position and Target position and posture of the posture information as tool in subsequent time, and according to control reference point Ref-A-next position and Posture information finds out tool in current time position and position and attitude misalignment of the tool between the target position of subsequent time Information；Wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool, is located at current time The tool of position is moved to subsequent time target position after interval time t；

The wherein specific solution procedure of position and attitude misalignment information are as follows: the tool for being set in current target position P point is sat Mark system is P-X-tool ' Y-tool ' Z-tool ',

Position and posture information, the target position that makes tool of the actuator according to control reference point Ref-A-next in 5th step Set the target position that subsequent time is moved to by current target position.

The specific expansion of other steps, each step of Trajectory Tracking Control method and the rail for realizing Trajectory Tracking Control method Mark tracking system is identical as embodiment seven, repeats no more.

The above is only presently preferred embodiments of the present invention, is not intended to limit the invention in any other form, and Any modification made by according to the technical essence of the invention or equivalent variations, still fall within the scope of protection of present invention.

Trajectory Tracking System described in embodiment one, embodiment two, embodiment three and example IV is to sensor-based system It is required that it is relatively low, even if sensor-based system scarce capacity leads to the height tolerance that cannot provide position while welding and workpiece inclined degree And cannot obtain perfectly track cross sectional shape or cannot frequently, be quickly obtained track cross sectional shape, rail can also be completed Mark tracks purpose.

Such as the sensor in sensor-based system use 2D camera, at this time sensor-based system be only capable of provide position while welding left and right it is inclined Poor information, when the height tolerance and workpiece inclined degree of position while welding cannot be provided, P-s=[Xs, 0, Zs], wherein Zs The value that can be assumed that rational position when no height tolerance, rather than detect, is only fixed value.Assuming that sensor coordinates system and tool The Z axis of coordinate system is parallel, then N-s=[0,0,1].

Because δ x is x=0 δ due to caused by height Zs variation；

Because δ y is y=0 δ due to caused by N-s variation；

Because δ z is z ≠ 0 δ due to caused by Xs variation.

So transforming to tool coordinates system, calculated deviation:

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

So, as long as track is in a plane perpendicular to Z-tool, i.e., trajectory height is guaranteed by equipment oneself, just still It can so accurately track.The case where similar sensor-based system scarce capacity it is as shown in figure 14 with cartesian co-ordinate type operation machine tracking There is no the plane curve of camber deviation Zs, the curve of camber variation can be ignored on tracking revolution shown in figure 15 cylindrical surface.

Cannot frequently, be quickly obtained track cross sectional shape, can only obtain one or extremely limited several track sections The sensor-based system of the case where shape, such as sensor in sensor-based system use displacement sensor, displacement sensor be it is a kind of only A point is measured to the equipment of sensor distance in side, and common laser displacement sensor is exactly its typical representative.With such biography Sensor tracks a straight weld in plane, but due to welding fixture inaccuracy, the position of weld seam, posture cannot be accurate enough Ground determines.Using method of the invention, still can be accurately tracked.Way is that sensor is attached on welding gun, with weldering Rifle has known geometric relationship.Welding gun is first inswept in welded joints with it when work, then one can be obtained in the starting point of weld seam Position while welding on a section.So just there is Q1-s and constant N1-s, it is assumed that the advance side of known weld seam To as long as can be welded with constant posture then adjust Tt in starting point.If the direction of advance of weld seam is not known yet Road obtains its information Q2-s, N2-s then can carry out a cross-sectional scans again in the terminal of weld seam or close to terminal point.Its Middle N2-s=N1-s, because straight welding line track posture is constant.The direction for determining Y-tool axis jointly with Q2-s and Q1-s, into one The direction of step Y-tool axis and N1-s vector determine the Tt of starting point.It is possible to complete to track with following two step:

Step 1: welding gun is adjusted to Tt,

Step 2: circulation reaches terminal all with identical activation bit D-tool=[X0,0,0,0,0,0] every time.

As above-mentioned two example examples, according to the actual situation, it can be supplemented out in Q-s, N-s, D-tool by analysis Element, then can be using the tracking that provide of the present invention, to simplifying, to execute equipment be significantly for this.At this moment There is following rule:

As long as z=0 △ in D-tool, what can be tracked is exactly plane or expandable curved face (circular conical surface, cylindrical surface face such as revolution) On curvilinear path expandable curved face is converted into equipment such as positioners as far as possible for workpiece；Such as z ≠ 0 △, but will execute When device is constrained to δ x=0, the track of slowly fluctuations can be still tracked；If N-s is not definite value, but actuator limits When at δ y=0, the track of lateral beat variation by a small margin can be still tracked；If a certain parameter cannot be detected, actuator is just used Structure guarantee its variation without departing from specified value.

Requirement of the Trajectory Tracking System described in embodiment one to embodiment eight to the system of execution is also relatively low, even if executing System is that insufficient DOF system could be used that trace tracking method of the present invention.

Shown in Figure 16, actuator is a 6DOF robot, and sensor is mounted on the 6th joint, welding gun is mounted on Situation on 5th joint.May also welding gun be mounted on joint 6 really, but the variation in joint 6 for welding gun characteristic point P without shadow It rings, but needs the Y-axis of sensor coordinate system and the Y-tool axis of the tool coordinates system angle, θ that is staggered that can just detect weldering Seam.Also that is, during tracking, the relativeness between sensor coordinate system and tool coordinates system is variation.At this moment it only needs System used in transformed coordinate system is distinguish, rather than broadly uses Tt.The example also reveal welding gun and One feature of some coating guns: the posture of tool does not need to adjust completely, then one revolution freedom degree of missing can be used The actuators of 5 freedom degrees track any space tracking, need to only guarantee that track during tracking falls in the inspection of sensor always Within the scope of survey.The embodiment is a kind of example for expanding sensor-based system detection range well.

It is insufficient DOF when the system of execution is less than 6 number of degrees of freedom,s, theoretically cannot must accurately tracks arbitrary Curve, but still can release improved method according to above-mentioned control principle, class and be used for specified conditions, belong to this patent guarantor Shield range.Such as the above-mentioned actuator with 5 freedom degrees drives the case where welding gun tracking any space tracking.Such as with only having The actuator of two or three translation freedoms, so that it may track the curve in plane.Situation shown in Figure 17 can track inclination Non-deflecting clinoplain on track；If actuator does not have the freedom degree of lift adjustment, it still is able to tracking level and puts The track in plane set.Such as shown in figure 18, the actuator of two degrees of freedom configures the positioner of a flip horizontal, can To track the curve on revolving body.Such as shown in figure 19, the actuator of three translation freedoms configures a vertical rotating shaft Positioner, track eccentric or not regular enough a toroid etc. on table top.So-called positioner is that one kind makes workpiece and machine The device of tool arm generation relative motion.If ignoring small posture of welding torch deviation, the actuators of these insufficient DOFs can also be with The more more complicated track of track, is not listed one by one.

It is described control system itself in preceding embodiment 1 to the contribution problem of track following, can also embody herein.Example Such as, in the case where y=0 forward drive component △ in D-tool, it is necessary in execution system, such as " teaching track " or " planned trajectory " mends out the one-component of at least y ≠ 0 △.

Certainly, similar deviation information combines the combination being not limited between such two groups of deviation informations, can also have multiple Deviation information source, and their statement reference frame may also be different, it can the same ginseng is arrived by the way that transformation is unified In examining and being, it can also be corrected step by step under different referentials respectively, finally obtain comprehensive control effect.

Sensor-based system 5, execution system 6 is not particularly limited in control method provided by the invention, but between system Can mutual cooperation, it is desirable that according to respective unique function, does corresponding information adjustment, such as cannot perceive or control Parameter definition be to react the standard value of actual conditions, therefore this control method can be widely applied.The present invention provides Trajectory Tracking System, a variety of different sensor-based systems 5, execution system 6 can be supported, as long as according to regular group of information exchange Build tracking system, so that it may select simple as far as possible or even incomplete device, complete specific tasks abundant；It can also be finished Kind sensor-based system 5 cooperates the execution system 6 of full freedom degree or redundant degree of freedom, completes extremely complex tracing task.Furthermore Trajectory Tracking System is further divided into sensor-based system and executes system two parts, the foundation of tracking system can be made more just It is sharp, flexible, and convenient for being manufactured by different specialized factories, it improves performance, reduce cost.

Claims

1. Trajectory Tracking Control method, it is characterised in that: tool is attached on actuator, and sensor is attached to the joint of actuator Upper or sensor is attached directly on tool, and the relative position between sensor and tool has determining geometrical relationship, tool Body the following steps are included:

(4) determine target: determined according to track position information set { Q } tool in the target position of subsequent time, specifically such as Under:

If tool is P point in the target position at current time, the tool coordinates system of P point is P-X-tool ' Y-tool ' Z- Tool ' crosses P point and does the plane vertical with track characteristic line, and the intersection point of the plane and track characteristic line is reference point Ref-A, with Reference point Ref-A is tracking target, using the position of reference point Ref-A as tool in the target position of subsequent time；

(6) above steps is recycled, until completing the tracking of whole track.

2. Trajectory Tracking Control method described in accordance with the claim 1, it is characterised in that: step is believed in (4) according to track position Breath set { Q } determines target position and posture of the tool in subsequent time, specific as follows:

If tool is P point in the target position at current time, the tool coordinates system of P point is P-X-tool ' Y-tool ' Z- Tool ' crosses P point and does the plane vertical with track characteristic line, and the intersection point of the plane and track characteristic line is reference point Ref-A, with Reference point Ref-A is tracking target, determines the tool coordinates system P-X-toolY-toolZ-tool being located at reference point Ref-A: The tangent line that reference point Ref-A makees track characteristic line is crossed, it is parallel with Z-tool ' axis straight with tangent line and the mistake of crossing track characteristic line The plane of line is track divisional plane；It crosses reference point Ref-A and does plane perpendicular to track characteristic line, the plane and track divisional plane Intersection be Z-tool axis, the positive direction of Z-tool axis is directed toward workpiece surface；It is to cross the tangent line of the track characteristic line of Ref-A point The positive direction of Y-tool axis, Y-tool axis is directed toward tool direction of advance；It is determined according to the left hand rule of coordinate system or the right-hand rule The positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture as tool subsequent time target Position and posture；

In step (5) actuator according to the position of reference point Ref-A and posture information, make the target position of tool by current time Target position move to Ref-A point, and be that starting point along the positive direction of Y-tool axis moves forward a step pitch △ using Ref-A =V*t。

3. Trajectory Tracking Control method described in accordance with the claim 1, it is characterised in that: with one and Z-tool ' in step (2) Posture information of the identical vector of axis direction as test point, and the posture information is expressed with sensor coordinate system；

The posture information expressed with sensor coordinate system is converted in step (3) appearance expressed with coordinate system accompanying by track Then state information is marked in coordinate system accompanying by track, and be stored in formation track posture information collection in information storage area It closes { N }, track posture information set and track position information set collectively form track position posture information set { Q, N }；

In step (4), according to track position posture information set { Q, N }, the tool coordinates system being located at reference point Ref-A is determined P-X-toolY-toolZ-tool: it crosses reference point Ref-A and does the plane perpendicular to track characteristic line, the plane and track divisional plane Intersection be Z-tool axis, the positive direction of Z-tool axis is directed toward workpiece surface；It is to cross the tangent line of the track characteristic line of Ref-A point The positive direction of Y-tool axis, Y-tool axis is directed toward tool direction of advance；It is determined according to the left hand rule of coordinate system or the right-hand rule The positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture information as tool in subsequent time Target position and posture；

4. according to Trajectory Tracking Control method described in claim 2 or 3, it is characterised in that: according to track position in step (4) Information aggregate { Q } determines that tool is as follows in the target position of subsequent time and the specific method of determination of posture:

Using reference point Ref-A as the centre of sphere, spherical surface is done by radius of △=V*t, there are two intersection points for the spherical surface and track characteristic line, take The intersection point of spherical surface and track characteristic line in tool direction of advance is control reference point Ref-A-next, to control reference point Ref-A-next is tracking target, determines the tool coordinates system P-X-tool " Y- being located at control reference point Ref-A-next Tool " Z-tool ": it crosses control reference point Ref-A-next and does the plane perpendicular to track characteristic line, the plane and track are divided The intersection in face is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；Control reference point Ref-A-next's to cross The tangent line of track characteristic line is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool direction of advance；According to a left side for coordinate system Hand rule or the right-hand rule determine the positive direction of X-tool " axis and X-tool " axis；The position of reference point Ref-A-next will be controlled And target position and posture of the posture information as tool in subsequent time；

Position and posture information, the target position that makes tool of the actuator according to control reference point Ref-A-next in step (5) Subsequent time target position is moved to by the target position at current time.

5. Trajectory Tracking Control method, it is characterised in that: tool is attached on actuator, and sensor is attached to the joint of actuator Upper or sensor is attached directly on tool, and the relative position between sensor and tool has determining geometrical relationship, tool Body the following steps are included:

(4) it determines target: determining target position and posture of the tool in subsequent time according to track position information set { Q }, And find out tool in position between subsequent time target position of current target position and tool and attitude misalignment information, The position and attitude misalignment information be lateral deviation △ x or lateral deviation △ x and progressive deviation △ y, height tolerance △ z, At least one of pitch angle deviation δ x, direction of advance angular deviation δ z are combined；Wherein according to track position information set { Q } determines that tool is as follows in the target position of subsequent time and the specific method of determination of posture:

If tool is P point in the target position at current time, the tool coordinates system of P point is P-X-tool ' Y-tool ' Z- Tool ' crosses P point and does the plane vertical with track characteristic line, and the intersection point of the plane and track characteristic line is reference point Ref-A, with Reference point Ref-A is tracking target, determines the tool coordinates system P-X-toolY-toolZ-tool being located at reference point Ref-A: The tangent line that reference point Ref-A makees track characteristic line is crossed, it is parallel with Z-tool ' axis straight with tangent line and the mistake of crossing track characteristic line The plane of line is track divisional plane；It crosses reference point Ref-A and does plane perpendicular to track characteristic line, the plane and track divisional plane Intersection be Z-tool axis, the positive direction of Z-tool axis is directed toward workpiece surface；It is to cross the tangent line of the track characteristic line of Ref-A point The positive direction of Y-tool axis, Y-tool axis is directed toward tool direction of advance；It is determined according to the left hand rule of coordinate system or the right-hand rule The positive direction of X-tool axis and X-tool axis；Using the position of reference point Ref-A and posture information as tool in subsequent time Target position and posture；

(5) driving instrument: actuator according to the position of reference point Ref-A and attitude misalignment information, make the target position of tool by The target position at current time moves to Ref-A point, and moves forward a step pitch △ along predetermined direction using Ref-A as starting point =V*t, wherein t is the adjustment interval time of each position correction control of tool, and V is the forward speed of tool；

The target position of tool is set to move to the amendment activation bit activation bit of Ref-A point by the target position at current time D-tool, activation bit D-tool are to contain at least one active constituent: the vector of △ x, and activation bit D-tool is at most Include five active constituents: the vector of △ x, △ y, △ z, δ x, δ z；

Wherein when activation bit D-tool does not include δ x or δ z, the predetermined direction is naturally true due to adjustment of displacement Fixed direction；When activation bit D-tool includes δ x or δ z, the predetermined direction is the positive direction of Y-tool axis；

(6) above steps is recycled, until completing the tracking of whole track.

6. Trajectory Tracking Control method according to claim 5, it is characterised in that: position and attitude misalignment in step (4) The specific solution procedure of information are as follows: set the tool coordinates system of current target position P point as P-X-tool ' Y-tool ' Z- Tool ',

7. Trajectory Tracking Control method according to claim 5, it is characterised in that: step is believed in (4) according to track position Breath gathers { Q } to determine that tool is as follows in the target position of subsequent time and the specific method of determination of posture:

Using reference point Ref-A as the centre of sphere, spherical surface is done by radius of △=V*t, there are two intersection points for the spherical surface and track characteristic line, take The intersection point of spherical surface and track characteristic line in tool direction of advance is control reference point Ref-A-next, to control reference point Ref-A-next is tracking target, determines the tool coordinates system P-X-tool " Y- being located at control reference point Ref-A-next Tool " Z-tool ": it crosses control reference point Ref-A-next and does the plane perpendicular to track characteristic line, the plane and track are divided The intersection in face is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；Control reference point Ref-A-next's to cross The tangent line of track characteristic line is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool direction of advance；According to a left side for coordinate system Hand rule or the right-hand rule determine the positive direction of X-tool " axis and X-tool " axis；The position of reference point Ref-A-next will be controlled And target position and posture of the posture information as tool in subsequent time；And according to the position of control reference point Ref-A-next And posture information finds out tool in position between subsequent time target position of current target position and tool and posture Deviation information, the position and attitude misalignment information are lateral deviation △ x or lateral deviation △ x and progressive deviation △ y, height At least one of deviation △ z, pitch angle deviation δ x, direction of advance angular deviation δ z are combined；

8. Trajectory Tracking Control method according to claim 7, it is characterised in that: position and attitude misalignment in step (4) The specific solution procedure of information are as follows: set the tool coordinates system of current target position P point as P-X-tool ' Y-tool ' Z- Tool ',

9. according to Trajectory Tracking Control method described in claim 5,6 or 7, it is characterised in that: the amendment driving in step (5) Information is the comprehensive driving that activation bit D-tool is combined at least one of scheduled task trace information, intervention information Information.

10. Trajectory Tracking Control method, sensor can perceive the location information and posture of track, it is characterised in that: tool attachment In on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor and tool Between relative position there is determining geometrical relationship, specifically includes the following steps:

(6) above steps is recycled, until completing the tracking of whole track.

11. Trajectory Tracking Control method according to claim 10, it is characterised in that: according to track position in step (4) Posture information set { Q, N } determines that tool is as follows in the target position of subsequent time and the specific method of determination of posture:

Position and posture information, the target position that makes tool of the actuator according to control reference point Ref-A-next in step (5) The target position of subsequent time is moved to by the target position at current time.

12. according to Trajectory Tracking Control method described in claim 10 or 11, it is characterised in that: will be with sensing in step (3) The position of device coordinate system expression and posture information are converted into the position of the expression of coordinate system accompanying by track and posture information Conversion process are as follows: first convert the position and posture information expressed with sensor coordinate system to the position expressed with tool coordinates system And posture information, then convert the position and posture information expressed with tool coordinates system to coordinate system table accompanying by track The position reached and posture information.

13. Trajectory Tracking Control method, sensor can perceive the location information and posture of track, it is characterised in that: tool attachment In on actuator, sensor is attached on the joint of actuator or sensor is attached directly on tool, and sensor and tool Between relative position there is determining geometrical relationship, specifically includes the following steps:

(4) determine target: determined according to track position posture information set { Q, N } tool in the target position of subsequent time, And tool is found out in position between the target of subsequent time of current target position and attitude and tool and attitude misalignment letter Breath, the position and attitude misalignment information are lateral deviation △ x, progressive deviation △ y, height tolerance △ z, pitch angle deviation δ x, lateral deviation angular displacement δ y, at least x containing △ in direction of advance angular deviation δ z this six active constituents, two active constituents of δ y Position deviation information, and it includes △ x that the position and attitude misalignment information, which is at most, △ y, △ z, δ x, δ y, δ z six is effectively The position deviation information of component；Wherein determine tool in subsequent time according to according to track position posture information set { Q, N } The specific method of determination of target position and posture is as follows:

(5) driving instrument: actuator makes the target position of tool by working as according to the position reference point Ref-A and attitude misalignment information Preceding moment target position moves to the target position of subsequent time；

The target position of tool is set to move to the amendment activation bit of the target position of subsequent time by current target position It is including at least two active constituents for activation bit D-tool, activation bit D-tool: the vector of △ x, δ y, and activation bit D-tool includes at most six active constituents: the vector of △ x, △ y, △ z, δ x, δ y, δ z；

(6) above steps is recycled, until completing the tracking of whole track.

14. Trajectory Tracking Control method according to claim 13, it is characterised in that: position and posture are inclined in step (4) The specific solution procedure of poor information are as follows: the tool coordinates system for being set in current target position P point is P-X-tool ' Y- Tool ' Z-tool ',

X is: projected length of the vector of P point to reference point Ref-A in X-tool ' axis direction；

15. Trajectory Tracking Control method according to claim 13, it is characterised in that: according to according to track position in step (4) Posture information set { Q, N } is set to determine that tool is as follows in the target position of subsequent time and the specific method of determination of posture:

Using reference point Ref-A as the centre of sphere, spherical surface is done by radius of △=V*t, there are two intersection points for the spherical surface and track characteristic line, take The intersection point of spherical surface and track characteristic line in tool direction of advance is control reference point Ref-A-next, to control reference point Ref-A-next is tracking target, determines the tool coordinates system P-X-tool " Y- being located at control reference point Ref-A-next Tool " Z-tool ": it crosses control reference point Ref-A-next and does the plane perpendicular to track characteristic line, the plane and track are divided The intersection in face is Z-tool " axis, and the positive direction of Z-tool " axis is directed toward workpiece surface；Control reference point Ref-A-next's to cross The tangent line of track characteristic line is Y-tool " axis, and the positive direction of Y-tool " axis is directed toward tool direction of advance；According to a left side for coordinate system Hand rule or the right-hand rule determine the positive direction of X-tool " axis and X-tool " axis；The position of reference point Ref-A-next will be controlled And target position and posture of the posture information as tool in subsequent time, and according to the position of control reference point Ref-A-next And posture information finds out tool in position between the target position of subsequent time of current target position and tool and appearance State deviation information；Wherein t is the adjustment interval time of tool each position correction control, and V is the forward speed of tool, positioned at working as The tool of preceding moment position is moved to subsequent time target position after interval time t；

Position and posture information, the target position that makes tool of the actuator according to control reference point Ref-A-next in step (5) The target position of subsequent time is moved to by current target position.

16. Trajectory Tracking Control method according to claim 15, it is characterised in that: position and posture are inclined in step (4) The specific solution procedure of poor information are as follows: set the tool coordinates system of the target position P point at tool current time as P-X-tool ' Y- Tool ' Z-tool ',

17. according to Trajectory Tracking Control method described in claim 13,14 or 15, it is characterised in that: the amendment in step (5) Activation bit is the synthesis that activation bit D-tool is combined at least one of scheduled task trace information, intervention information Activation bit.

18. according to Trajectory Tracking Control method described in claim 13,14 or 15, it is characterised in that: will be to pass in step (3) The position and attitude information of sensor coordinate system expression is converted into be turned with the position and attitude information of the expression of coordinate system accompanying by track Change process are as follows: first convert the position and attitude information expressed with sensor coordinate system to the position and attitude expressed with tool coordinates system Then information converts the position and attitude information expressed with tool coordinates system to the position expressed with coordinate system accompanying by track Posture information.

19. realize claim 1-4 in any Trajectory Tracking Control method Trajectory Tracking System, including sensor, tool and Actuator, tool are attached on actuator, it is characterised in that: sensor is attached on the joint of actuator or sensor is directly attached There is determining geometrical relationship in the relative position on tool, and between sensor and tool；It further include infomation detection processing Device, Information application processor, positive kinematics module and inverse kinematics module further include information storage in Information application processor Area；

Sensor acquires the initial trace information of track on workpiece；

20. Trajectory Tracking System according to claim 19, it is characterised in that: sensor and infomation detection processor are constituted Sensor-based system is provided with the first correspondence with foreign country device and first information interface end in sensor-based system；Information application processor, positive fortune Dynamic module, inverse kinematics module and the actuator learned constitutes execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system is passed down to execution system with the location information of the track of sensor coordinate system expression.

21. Trajectory Tracking System according to claim 19, it is characterised in that: sensor, infomation detection processor and letter It ceases application processor and constitutes sensor-based system, the first correspondence with foreign country device and first information interface end are provided in sensor-based system；Just Kinematics module, inverse kinematics module and actuator constitute execution system, and the second correspondence with foreign country device is provided in execution system With the second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end； Sensor-based system receives target position and the posture information at the tool current time that execution system uploads；Sensor-based system to execute system The lower target position for passing tool subsequent time and posture information.

22. Trajectory Tracking System according to claim 19, it is characterised in that: sensor, infomation detection processor, information Application processor, positive kinematics module composition sensor-based system are provided with the first correspondence with foreign country device and the first letter in sensor-based system Cease interface end；Inverse kinematics module and actuator constitute execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads； Sensor-based system passes down target position and the posture information of tool subsequent time to execution system.

23. Trajectory Tracking System according to claim 19, it is characterised in that: sensor, infomation detection processor, information It is externally logical to be provided with first in sensor-based system for application processor, positive kinematics module, inverse kinematics module composition sensor-based system Believe device and first information interface end；Actuator constitutes execution system, and the second correspondence with foreign country device and the are provided in execution system Two information interface ends, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；Sensing System receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads；It passes Sensing system passes down the joint angle information of actuator when tool subsequent time reaches target position and posture to execution system.

24. realize claim 5-9 in any Trajectory Tracking Control method Trajectory Tracking System, including sensor, tool and Actuator, tool are attached on actuator, it is characterised in that: sensor is attached on the joint of actuator or sensor is directly attached There is determining geometrical relationship in the relative position on tool, and between sensor and tool；It further include infomation detection processing Device, Information application processor, positive kinematics module and inverse kinematics module further include information storage in Information application processor Area；

Sensor acquires the initial trace information of track on workpiece；

Information application processor calls the target position at calculated tool current time and posture information in positive kinematics module, And determine tool next with posture information and track position information set { Q } according to the target position at tool current time The target position at moment and posture information, and find out tool current target position and tool subsequent time target it Between position and attitude misalignment information；

Calculated tool is in current target position and tool under in inverse kinematics module recalls information application processor Position and attitude misalignment information between the target position at one moment, and existed according to tool in current target position and tool Position and attitude misalignment information between the target position of subsequent time, the target position for calculating tool are located at subsequent time mesh The joint angle information of actuator when cursor position.

25. according to the Trajectory Tracking System described in claim 24, it is characterised in that: sensor and infomation detection processor are constituted Sensor-based system is provided with the first correspondence with foreign country device and first information interface end in sensor-based system；Information application processor, positive fortune Dynamic module, inverse kinematics module and the actuator learned constitutes execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system is passed down to execution system with the location information of the track of sensor coordinate system expression.

26. according to the Trajectory Tracking System described in claim 24, it is characterised in that: sensor, infomation detection processor and letter It ceases application processor and constitutes sensor-based system, the first correspondence with foreign country device and first information interface end are provided in sensor-based system；Just Kinematics module, inverse kinematics module and actuator constitute execution system, and the second correspondence with foreign country device is provided in execution system With the second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end； Sensor-based system receives target position and the posture information at the tool current time that execution system uploads；Sensor-based system to execute system The lower target position for passing tool subsequent time and posture information.

27. according to the Trajectory Tracking System described in claim 24, it is characterised in that: sensor, infomation detection processor, information Application processor, positive kinematics module composition sensor-based system are provided with the first correspondence with foreign country device and the first letter in sensor-based system Cease interface end；Inverse kinematics module and actuator constitute execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads； Sensor-based system passes down target position and the posture information of tool subsequent time to execution system.

28. according to the Trajectory Tracking System described in claim 24, it is characterised in that: sensor, infomation detection processor, information It is externally logical to be provided with first in sensor-based system for application processor, positive kinematics module, inverse kinematics module composition sensor-based system Believe device and first information interface end；Actuator constitutes execution system, and the second correspondence with foreign country device and the are provided in execution system Two information interface ends, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；Sensing System receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads；It passes Sensing system passes down the joint angle information of actuator when tool subsequent time reaches target position and posture to execution system.

29. realizing the Trajectory Tracking System of any Trajectory Tracking Control method in claim 10-12, including sensor, tool And actuator, sensor can perceive the location information and posture of track, tool is attached on actuator, it is characterised in that: sensing Device is attached on the joint of actuator or sensor is attached directly on tool, and the relative position tool between sensor and tool There is determining geometrical relationship；It further include infomation detection processor, Information application processor, positive kinematics module and inverse kinematics mould Block further includes information storage area in Information application processor；

Sensor acquires the initial trace information of track on workpiece；

30. according to the Trajectory Tracking System described in claim 29, it is characterised in that: sensor and infomation detection processor are constituted Sensor-based system is provided with the first correspondence with foreign country device and first information interface end in sensor-based system；Information application processor, positive fortune Dynamic module, inverse kinematics module and the actuator learned constitutes execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system is passed down to execution system with the location information of the track of sensor coordinate system expression.

31. according to the Trajectory Tracking System described in claim 29, it is characterised in that: sensor, infomation detection processor and letter It ceases application processor and constitutes sensor-based system, the first correspondence with foreign country device and first information interface end are provided in sensor-based system；Just Kinematics module, inverse kinematics module and actuator constitute execution system, and the second correspondence with foreign country device is provided in execution system With the second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end； Sensor-based system receives target position and the posture information at the tool current time that execution system uploads；Sensor-based system to execute system The lower target position for passing tool subsequent time and posture information.

32. according to the Trajectory Tracking System described in claim 29, it is characterised in that: sensor, infomation detection processor, information Application processor, positive kinematics module composition sensor-based system are provided with the first correspondence with foreign country device and the first letter in sensor-based system Cease interface end；Inverse kinematics module and actuator constitute execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads； Sensor-based system passes down target position and the posture information of tool subsequent time to execution system.

33. according to the Trajectory Tracking System described in claim 29, it is characterised in that: sensor, infomation detection processor, information It is externally logical to be provided with first in sensor-based system for application processor, positive kinematics module, inverse kinematics module composition sensor-based system Believe device and first information interface end；Actuator constitutes execution system, and the second correspondence with foreign country device and the are provided in execution system Two information interface ends, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；Sensing System receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads；It passes Sensing system passes down the joint angle information of actuator when tool subsequent time reaches target position and posture to execution system.

34. realizing the Trajectory Tracking System of any Trajectory Tracking Control method in claim 13-18, including sensor, tool And actuator, sensor can perceive the location information and posture of track, tool is attached on actuator, it is characterised in that: sensing Device is attached on the joint of actuator or sensor is attached directly on tool, and the relative position tool between sensor and tool There is determining geometrical relationship；It further include infomation detection processor, Information application processor, positive kinematics module and inverse kinematics mould Block further includes information storage area in Information application processor；

Sensor acquires the initial trace information of track on workpiece；

Information application processor calls the position at calculated tool current time and posture information in positive kinematics module, will be with Sensor coordinate system expression track position and posture information be converted into coordinate system accompanying by track expression position and Then posture information is marked in coordinate system accompanying by track, and be stored in formation track position posture in information storage area Information aggregate { Q, N }；

Information application processor is according to the target position at tool current time and posture information and track position posture information collection It closes { Q, N }, determines target position and posture of the tool in subsequent time, and find out tool in current target position and tool Position and attitude misalignment information between the target position of subsequent time；

35. according to the Trajectory Tracking System described in claim 34, it is characterised in that: sensor and infomation detection processor are constituted Sensor-based system is provided with the first correspondence with foreign country device and first information interface end in sensor-based system；Information application processor, positive fortune Dynamic module, inverse kinematics module and the actuator learned constitutes execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system is passed down to execution system with the location information of the track of sensor coordinate system expression.

36. according to the Trajectory Tracking System described in claim 34, it is characterised in that: sensor, infomation detection processor and letter It ceases application processor and constitutes sensor-based system, the first correspondence with foreign country device and first information interface end are provided in sensor-based system；Just Kinematics module, inverse kinematics module and actuator constitute execution system, and the second correspondence with foreign country device is provided in execution system With the second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end； Sensor-based system receives target position and the posture information at the tool current time that execution system uploads；Sensor-based system to execute system The lower target position for passing tool subsequent time and posture information.

37. according to the Trajectory Tracking System described in claim 34, it is characterised in that: sensor, infomation detection processor, information Application processor, positive kinematics module composition sensor-based system are provided with the first correspondence with foreign country device and the first letter in sensor-based system Cease interface end；Inverse kinematics module and actuator constitute execution system, be provided in execution system the second correspondence with foreign country device and Second information interface end, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；It passes Sensing system receives the joint angle information of the target position actuator at current target position for the tool that execution system uploads； Sensor-based system passes down target position and the posture information of tool subsequent time to execution system.

38. according to the Trajectory Tracking System described in claim 34, it is characterised in that: sensor, infomation detection processor, information It is externally logical to be provided with first in sensor-based system for application processor, positive kinematics module, inverse kinematics module composition sensor-based system Believe device and first information interface end；Actuator constitutes execution system, and the second correspondence with foreign country device and the are provided in execution system Two information interface ends, sensor-based system and execution system are electrically connected by first information interface end and the second information interface end；Sensing System receives the joint angle information of the target position actuator at the target position at current time for the tool that execution system uploads； Sensor-based system passes down the joint angle information of actuator when tool subsequent time reaches target position and posture to execution system.