CN110450150A - Trajectory Tracking Control method and Trajectory Tracking System - Google Patents
Trajectory Tracking Control method and Trajectory Tracking System Download PDFInfo
- Publication number
- CN110450150A CN110450150A CN201810424413.0A CN201810424413A CN110450150A CN 110450150 A CN110450150 A CN 110450150A CN 201810424413 A CN201810424413 A CN 201810424413A CN 110450150 A CN110450150 A CN 110450150A
- Authority
- CN
- China
- Prior art keywords
- tool
- information
- track
- sensor
- target position
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
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
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:
(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) 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.
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 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;
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, 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:
(1) it detects track: acquiring the initial trace information of test point on the track on workpiece by sensor;
(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;
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 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:
(1) it detects track: acquiring the initial trace information of test point on the track on workpiece by sensor;
(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, 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 ',
△ 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;
δ 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 ',
△ 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;
δ 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;
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;
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 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 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: Q1、Q2、Q3……Qi.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 2: extracting trace information: extracting the location information of test point, and from initial trace information with sensor
Coordinate system expresses the location information.
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.
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 ',
△ 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.
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;
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 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 ',
δ 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.
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.
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.
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)
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.
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 ',
△ 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;
δ 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.
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;
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 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.
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.
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 ',
△ 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;
δ 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.
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 (38)
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:
(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:
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;
(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.
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;
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。
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:
(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) 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 ',
δ 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.
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;
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.
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 ',
δ 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.
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:
(1) it detects track: acquiring the initial trace information of test point on the track on workpiece by sensor;
(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.
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:
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)
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:
(1) it detects track: acquiring the initial trace information of test point on the track on workpiece by sensor;
(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 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:
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 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;
△ 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;
δ 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.
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 ',
△ 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;
δ 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.
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;
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.
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;
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, 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;
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;
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 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.
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;
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;
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 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;
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.
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.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810424413.0A CN110450150B (en) | 2018-05-07 | 2018-05-07 | Trajectory tracking control method and trajectory tracking system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810424413.0A CN110450150B (en) | 2018-05-07 | 2018-05-07 | Trajectory tracking control method and trajectory tracking system |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110450150A true CN110450150A (en) | 2019-11-15 |
CN110450150B CN110450150B (en) | 2022-10-21 |
Family
ID=68471610
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810424413.0A Active CN110450150B (en) | 2018-05-07 | 2018-05-07 | Trajectory tracking control method and trajectory tracking system |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110450150B (en) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111251303A (en) * | 2020-03-11 | 2020-06-09 | 北京理工大学 | Robot motion control method for periodic attitude adjustment |
CN112059490A (en) * | 2020-08-18 | 2020-12-11 | 佛山博文机器人自动化科技有限公司 | Robot on-site welding device and welding method |
CN112257587A (en) * | 2020-10-22 | 2021-01-22 | 江苏禹空间科技有限公司 | Target object detection effect evaluation method and device, storage medium and equipment |
CN112936237A (en) * | 2021-02-26 | 2021-06-11 | 中国农业大学 | Force feedback control system and method of tapping robot and tapping robot |
CN114310063A (en) * | 2022-01-28 | 2022-04-12 | 长春职业技术学院 | Welding optimization method based on six-axis robot |
CN115890053A (en) * | 2023-03-02 | 2023-04-04 | 成都熊谷加世电器有限公司 | Internal welding machine aligning method and device, internal welding machine and storage medium |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5465037A (en) * | 1993-01-11 | 1995-11-07 | Huissoon; Jan P. | System and method for tracking a feature on an object using a redundant axis |
JP2003256012A (en) * | 2002-03-04 | 2003-09-10 | Japan Science & Technology Corp | Tool path plane calculating method, tool path plane calculating program and recording medium recording tool path plane calculating program |
US20110062118A1 (en) * | 2009-09-09 | 2011-03-17 | Agie Charmilles Sa | Method and machine for machining parts using spark-erosion milling |
CN103744349A (en) * | 2013-10-08 | 2014-04-23 | 华中科技大学 | Interference-free tool path generation method in machining of transitional surfaces by flat-end milling cutter |
KR20160149342A (en) * | 2015-06-17 | 2016-12-28 | 한국생산기술연구원 | A processing route generation method of rotation tool, tool path-generating device, machine tool and recording medium for processing program |
CN107247444A (en) * | 2017-05-22 | 2017-10-13 | 华中科技大学 | It is a kind of to be used to generate projection algorithm of the curved surface without interference five-axis robot track |
-
2018
- 2018-05-07 CN CN201810424413.0A patent/CN110450150B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5465037A (en) * | 1993-01-11 | 1995-11-07 | Huissoon; Jan P. | System and method for tracking a feature on an object using a redundant axis |
JP2003256012A (en) * | 2002-03-04 | 2003-09-10 | Japan Science & Technology Corp | Tool path plane calculating method, tool path plane calculating program and recording medium recording tool path plane calculating program |
US20110062118A1 (en) * | 2009-09-09 | 2011-03-17 | Agie Charmilles Sa | Method and machine for machining parts using spark-erosion milling |
CN103744349A (en) * | 2013-10-08 | 2014-04-23 | 华中科技大学 | Interference-free tool path generation method in machining of transitional surfaces by flat-end milling cutter |
KR20160149342A (en) * | 2015-06-17 | 2016-12-28 | 한국생산기술연구원 | A processing route generation method of rotation tool, tool path-generating device, machine tool and recording medium for processing program |
CN107247444A (en) * | 2017-05-22 | 2017-10-13 | 华中科技大学 | It is a kind of to be used to generate projection algorithm of the curved surface without interference five-axis robot track |
Non-Patent Citations (1)
Title |
---|
洪宇翔等: "基于轨迹动态规划的移动机器人焊道自动跟踪", 《焊接学报》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111251303A (en) * | 2020-03-11 | 2020-06-09 | 北京理工大学 | Robot motion control method for periodic attitude adjustment |
CN112059490A (en) * | 2020-08-18 | 2020-12-11 | 佛山博文机器人自动化科技有限公司 | Robot on-site welding device and welding method |
CN112257587A (en) * | 2020-10-22 | 2021-01-22 | 江苏禹空间科技有限公司 | Target object detection effect evaluation method and device, storage medium and equipment |
CN112257587B (en) * | 2020-10-22 | 2023-12-22 | 无锡禹空间智能科技有限公司 | Target object detection effect evaluation method, device, storage medium and equipment |
CN112936237A (en) * | 2021-02-26 | 2021-06-11 | 中国农业大学 | Force feedback control system and method of tapping robot and tapping robot |
CN112936237B (en) * | 2021-02-26 | 2024-04-19 | 中国农业大学 | Force feedback control system and method of rubber tapping robot and rubber tapping robot |
CN114310063A (en) * | 2022-01-28 | 2022-04-12 | 长春职业技术学院 | Welding optimization method based on six-axis robot |
CN114310063B (en) * | 2022-01-28 | 2023-06-06 | 长春职业技术学院 | Welding optimization method based on six-axis robot |
CN115890053A (en) * | 2023-03-02 | 2023-04-04 | 成都熊谷加世电器有限公司 | Internal welding machine aligning method and device, internal welding machine and storage medium |
CN115890053B (en) * | 2023-03-02 | 2023-08-18 | 成都熊谷加世电器有限公司 | Internal welding machine alignment method and device, internal welding machine and storage medium |
Also Published As
Publication number | Publication date |
---|---|
CN110450150B (en) | 2022-10-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110450150A (en) | Trajectory Tracking Control method and Trajectory Tracking System | |
CN112659123B (en) | Two-robot curve welding collaborative planning method based on line structured light vision | |
US8706300B2 (en) | Method of controlling a robotic tool | |
CN104400279B (en) | Pipeline space weld seam based on CCD identifies the method with trajectory planning automatically | |
CN112847353B (en) | Multi-segment welding seam track correction method based on offline programming software | |
CN113001535B (en) | Automatic correction system and method for robot workpiece coordinate system | |
US8504188B2 (en) | Device and method for the computer-assisted generation of a manipulator path | |
CN104827479A (en) | Pipeline insertion intersecting line track planning method orientated to laser machining robot | |
CN112958959A (en) | Automatic welding and detection method based on three-dimensional vision | |
CN114161048B (en) | 3D vision-based parameterized welding method and device for tower legs of iron tower | |
Fang et al. | Robot path and end-effector orientation planning using augmented reality | |
CN113223071B (en) | Workpiece weld joint positioning method based on point cloud reconstruction | |
CN111496428A (en) | Multilayer multi-pass welding bead planning method based on straight welding seam contour recognition and welding workstation | |
CN112958974A (en) | Interactive automatic welding system based on three-dimensional vision | |
CN112719702A (en) | Intelligent assembling and welding integrated device and method for complex pipeline | |
CN110456729A (en) | A kind of Trajectory Tracking Control method and Trajectory Tracking System | |
CN110455272A (en) | Sensor-based system in Trajectory Tracking System | |
CN117584121A (en) | Welding robot path planning method based on point cloud scene understanding | |
CN111702380A (en) | Welding process control method of welding robot | |
US20230103159A1 (en) | A High-Precision Mobile Robot Management and Scheduling System | |
KR102276050B1 (en) | Learning data creation system for vertical articulated robot | |
CN115723133A (en) | Robot space welding seam automatic positioning and deviation rectifying system based on virtual-real combination | |
CN113664831B (en) | Welding robot system and method for acquiring weld point cloud information | |
CN114237150A (en) | Robot welding seam milling path control method and device based on welding seam characteristics | |
CN115026843B (en) | Automatic TCP calibration and deviation correction method based on cross laser sensor |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |