CN114505865A - Pose tracking-based mechanical arm path generation method and system - Google Patents
Pose tracking-based mechanical arm path generation method and system Download PDFInfo
- Publication number
- CN114505865A CN114505865A CN202210250765.5A CN202210250765A CN114505865A CN 114505865 A CN114505865 A CN 114505865A CN 202210250765 A CN202210250765 A CN 202210250765A CN 114505865 A CN114505865 A CN 114505865A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- mechanical arm
- pose
- relative
- tool
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000005259 measurement Methods 0.000 claims description 44
- 238000012545 processing Methods 0.000 claims description 10
- 238000003754 machining Methods 0.000 claims description 5
- 230000001172 regenerating effect Effects 0.000 claims description 3
- 230000007547 defect Effects 0.000 description 4
- 239000003550 marker Substances 0.000 description 4
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000007935 neutral effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- 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
- B25J17/00—Joints
- B25J17/02—Wrist joints
- B25J17/0258—Two-dimensional joints
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
Abstract
The invention discloses a pose tracking-based mechanical arm path generation method and system. The method comprises the following steps: building a mechanical arm path tracking scene; determining a pose relationship between a workpiece and the mechanical arm in a working environment based on the path tracking scene of the mechanical arm; establishing a kinematic model based on the pose relationship between the workpiece and the mechanical arm; obtaining a mechanical arm motion joint angle corresponding to the motion along the expected track according to the inverse solution of the kinematic model; and compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path. The invention preliminarily determines the pose relation of the workpiece relative to the mechanical arm in the working environment, then realizes the accurate positioning of the target path by using an iterative learning control method, gradually improves the positioning precision of the path by repeated iterative learning, reduces the positioning error, and can effectively ensure the positioning precision of the whole target path.
Description
Technical Field
The invention relates to the technical field of mechanical arm path generation, in particular to a method and a system for generating a mechanical arm path based on pose tracking.
Background
The mechanical arm is used as a mechanical device for automatically executing tasks, and can move according to path point positions taught by people or programs compiled offline, so that corresponding work is completed. Its task is to replace the heavy work of human beings to realize the mechanization and automation of industry. When the mechanical arm executes a high-precision task, the control of the motion path of the mechanical arm is very critical. At present, the teaching and off-line programming of two basic modes of mechanical arm actual operation have obvious drawbacks. The cost of the teaching time of the mechanical arm is too high, and the teaching is manually performed point by professional personnel, so that the automation degree is obviously reduced. In application scenes facing to some complex continuous paths, teaching of the mechanical arm is more useless. Although offline programming increases the automation level of the mechanical arm, the actual motion path of the mechanical arm and the theoretical motion path of the mechanical arm usually have a larger positioning error due to the influence of machining errors, assembly errors, component wear, end load changes and temperature. This therefore adversely affects the wide application of the robot arm.
Disclosure of Invention
The invention aims to provide a pose tracking-based mechanical arm path generation method and a pose tracking-based mechanical arm path generation system, which are used for improving the positioning accuracy of a path, reducing the positioning error and effectively ensuring the positioning accuracy of the whole target path.
In order to achieve the purpose, the invention provides the following scheme:
a mechanical arm path generation method based on pose tracking comprises the following steps:
constructing a mechanical arm path tracking scene;
determining a pose relationship between a workpiece and the mechanical arm in a working environment based on the path tracking scene of the mechanical arm;
establishing a kinematic model based on the pose relationship between the workpiece and the mechanical arm;
obtaining a mechanical arm motion joint angle corresponding to the motion along the expected track according to the inverse solution of the kinematic model;
and compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path.
Optionally, the constructing a mechanical arm path tracking scene specifically includes:
fixing the mechanical arm provided with the cutter on a processing platform;
moving the pose tracking device to the right front of the mechanical arm;
and moving the mobile platform to the machining range of the mechanical arm, and simultaneously installing two markers on the clamp and the cutter respectively.
Optionally, determining a pose relationship between the workpiece and the mechanical arm in the working environment based on the path tracking scene of the mechanical arm specifically includes:
acquiring the pose relations of a tool coordinate system and a clamp coordinate system relative to a measurement coordinate system through a pose tracking device;
determining the pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system according to the positive kinematics of the mechanical arm;
determining the pose relationship of a tool coordinate system relative to a mechanical arm tail end coordinate system and the pose relationship of a TCP tool tail end coordinate system relative to the tool coordinate system based on the geometric dimension of the tool;
determining the pose relation of a workpiece coordinate system relative to a fixture coordinate system according to the geometric dimension of the fixture;
and determining the pose relation of the workpiece coordinate system relative to the stand coordinate system based on the pose relation of the tool coordinate system relative to the measuring coordinate system, the pose relation of the clamp coordinate system relative to the measuring coordinate system, the pose relation of the mechanical arm tail end coordinate system relative to the stand coordinate system, the pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system, the pose relation of the TCP tool tail end coordinate system relative to the tool coordinate system and the pose relation of the workpiece coordinate system relative to the clamp coordinate system.
Optionally, the establishing a kinematic model based on a pose relationship between the workpiece and the mechanical arm specifically includes:
determining the actual pose relationship of the tail end coordinate system of the mechanical arm relative to the base coordinate system;
determining the actual pose relation of a tool coordinate system relative to the terminal coordinate of the mechanical arm;
determining the actual pose relation of the machine base coordinate system relative to the measurement coordinate system based on the actual pose relation of the mechanical arm tail end coordinate system relative to the machine base coordinate system and the actual pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system;
and suggesting a kinematics model based on the actual pose relationship of the mechanical arm tail end coordinate system relative to the machine base coordinate system, the actual pose relationship of the tool coordinate system relative to the mechanical arm tail end coordinate system, the actual pose relationship of the machine base coordinate system relative to the measurement coordinate system and the pose relationship of the TCP tool tail end coordinate system relative to the tool coordinate system.
Optionally, compensating the motion joint angle of the mechanical arm by using an iterative learning control method, specifically including
Measuring positioning deviation through a pose tracking device in the motion process, inputting the positioning deviation into a controller, outputting joint compensation quantity of a current point on a path, putting the joint compensation quantity of the current point in an iteration memory, and defining the path tracking and positioning for the first time of the mechanical arm as first iteration; and then, carrying out second iteration, carrying out pose error compensation based on the first joint compensation amount, regenerating a path, measuring the positioning deviation by using a pose tracking device again, inputting the positioning deviation into the controller, outputting the joint compensation amount when the path is positioned for the second time, placing the joint compensation amount in an iteration memory, repeating the steps until the pose error of each point position on the whole path is smaller than the set error, stopping the iteration process, defining the motion joint angle of the mechanical arm generated by the last iteration as a final motion output form, and processing the workpiece.
The invention also provides a pose tracking-based mechanical arm path generation system, which comprises:
the scene building module is used for building a mechanical arm path tracking scene;
the pose relation determining module is used for determining the pose relation between the workpiece and the mechanical arm in the working environment based on the mechanical arm path tracking scene;
the kinematic model establishing module is used for establishing a kinematic model based on the pose relation between the workpiece and the mechanical arm;
the mechanical arm motion joint angle determining module is used for solving a mechanical arm motion joint angle corresponding to motion along an expected track according to the inverse solution of the kinematic model;
and the compensation module is used for compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path.
Optionally, the pose relationship determination module specifically includes:
the first pose relation determining unit is used for obtaining pose relations of the tool coordinate system and the clamp coordinate system relative to the measurement coordinate system through the pose tracking device;
the second pose relation determining unit is used for determining the pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system according to the positive kinematics of the mechanical arm;
the third pose relation determining unit is used for determining the pose relation of a tool coordinate system relative to the tail end coordinate system of the mechanical arm and the pose relation of the TCP tool tail end coordinate system relative to the tool coordinate system based on the geometric dimension of the tool;
the fourth pose relation determining unit is used for determining the pose relation of the workpiece coordinate system relative to the clamp coordinate system according to the geometric dimension of the clamp;
and the fifth pose relation determination unit is used for determining the pose relation of the workpiece coordinate system relative to the stand coordinate system based on the pose relation of the tool coordinate system relative to the measurement coordinate system, the pose relation of the clamp coordinate system relative to the measurement coordinate system, the pose relation of the tail end coordinate system of the mechanical arm relative to the stand coordinate system, the pose relation of the tool coordinate system relative to the tail end coordinate system of the mechanical arm, the pose relation of the tail end coordinate system of the TCP tool relative to the tool coordinate system and the pose relation of the workpiece coordinate system relative to the clamp coordinate system.
Optionally, the kinematic model building module specifically includes:
the first actual pose relation determining unit is used for determining the actual pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system;
the second actual pose relation determining unit is used for determining the actual pose relation of the tool coordinate system relative to the terminal coordinate of the mechanical arm;
the third actual pose relation determining unit is used for determining the actual pose relation of the machine base coordinate system relative to the measurement coordinate system based on the actual pose relation of the mechanical arm tail end coordinate system relative to the machine base coordinate system and the actual pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system;
and the kinematic model establishing unit is used for proposing a kinematic model based on the actual pose relationship of the mechanical arm tail end coordinate system relative to the machine base coordinate system, the actual pose relationship of the tool coordinate system relative to the mechanical arm tail end coordinate system, the actual pose relationship of the machine base coordinate system relative to the measurement coordinate system and the pose relationship of the TCP tool tail end coordinate system relative to the tool coordinate system.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention preliminarily determines the pose relation of the workpiece relative to the mechanical arm in the working environment, then realizes the accurate positioning of the target path by using an iterative learning control method, gradually improves the positioning precision of the path by repeated iterative learning, reduces the positioning error, and can effectively ensure the positioning precision of the whole target path. The invention has wide application field and application prospect, breaks through the defects of the conventional mechanical arm teaching and offline programming, and makes up the defects of the traditional control method.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a flow chart of a robot path generation method based on pose tracking according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of coordinate systems according to an embodiment of the present invention;
FIG. 3 illustrates a robot path tracking scenario constructed in accordance with an embodiment of the present invention;
FIG. 4 illustrates an x-direction error of a path tracking process according to an embodiment of the present invention;
FIG. 5 shows an error in the y-direction of a path tracking process according to an embodiment of the invention;
FIG. 6 shows an error in the z-direction of a path tracking process according to an embodiment of the invention;
FIG. 7 is a flowchart of a path tracing process according to an embodiment of the present invention
Error in direction;
FIG. 8 illustrates an error in the beta direction of a path tracking process according to an embodiment of the present invention;
FIG. 9 shows the gamma direction error of the path tracking process according to one embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention aims to provide a pose tracking-based mechanical arm path generation method and a pose tracking-based mechanical arm path generation system, which are used for improving the positioning accuracy of a path, reducing the positioning error and effectively ensuring the positioning accuracy of the whole target path.
In order to make the aforementioned objects, features and advantages of the present invention more comprehensible, the present invention is described in detail with reference to the accompanying drawings and the detailed description thereof.
As shown in fig. 1, the pose tracking-based robot arm path generation method provided by the invention comprises the following steps:
step 101: and constructing a mechanical arm path tracking scene.
As shown in fig. 3, a mechanical arm with a tool mounted thereon is fixed on a processing platform, a pose tracking device is moved to the front of the mechanical arm, after the positions of the mechanical arm and the pose tracking device are arranged, a fixture with a workpiece is fixed on a mobile platform, the mobile platform is moved to the processing range of the mechanical arm, and two markers (the pose tracking device can measure the position and the pose of the marker in space, the measurement principle is that the positions of a plurality of light sources or light reflecting original elements in space can be obtained through at least three light sources or light reflecting original elements on the marker, and finally the pose of the marker relative to the position of the pose tracking device in space is calculated) are respectively arranged on the fixture and the tool. A corresponding coordinate system is also established in fig. 2.
Step 102: and determining the pose relation between the workpiece and the mechanical arm in the working environment based on the path tracking scene of the mechanical arm.
Based on the established processing scene, the pose relation of the workpiece relative to the mechanical arm in the working environment needs to be determined so as to realize the processing of the target path. From the coordinate relationship in fig. 2, the following relationship can be determined
Wherein { M } represents a measurement coordinate system, { C } represents a TCP tool tip coordinate system, { B } represents a robot arm base coordinate system, { E } represents a robot arm tip coordinate system, { T } represents a tool coordinate system, { W } represents a workpiece coordinate system, { F } represents a chuck coordinate system; formula (1) represents the pose of the tool in space based on the measurement coordinate system, formula (2) represents the spatial pose of the mechanical arm controlling the motion of the TCP based on the measurement coordinate system, and formula (3) represents the spatial pose of the workpiece based on the measurement coordinate system. By using the pose tracking means, the poses of the tool coordinate system and the fixture coordinate system with respect to the measurement coordinate system, i.e. the pose
And
according to the mechanical armKinematics, determining the pose relationship of the robot arm end coordinate system relative to the frame coordinate system, i.e.
Based on the geometrical size of the tool, the pose relationship of the tool coordinate system relative to the robot arm end coordinate system and the TCP tool end coordinate system relative to the tool coordinate system can be determined, i.e.
And
depending on the geometry of the gripper, the pose relationship of the workpiece coordinate system relative to the gripper coordinate system can be determined, i.e.
The pose of the mechanical arm base under the measurement coordinate system can be obtained by the formula (1)
The formula (3) and the formula (4) are combined, and the pose relation of the workpiece coordinate system relative to the machine base coordinate system can be obtained
Based on the coordinate system of the workpiece, the path line to be processed is known, so the position and the posture of the path line to be processed relative to the coordinate system of the stand are
Where { P } represents the path coordinate system and the index i represents the ith target point location of the desired path. The final purpose of the path fine positioning is to control the TCP of the tool tip to follow a desired path, which can be expressed mathematically as
Step 103: and establishing a kinematic model based on the pose relationship between the workpiece and the mechanical arm.
After the pose relationship between a workpiece and a mechanical arm in a working environment is preliminarily determined, the pose relationship between a path line and a base coordinate system can be determined, a path can be planned through inverse solution of mechanical arm kinematics, however, due to the fact that corresponding positioning deviation exists in the system, the actual movement path and the expected movement path of a tail end tool TCP of the mechanical arm generally have large positioning errors, the system needs to be analyzed, and a system kinematics model is constructed. In the scene of accurate positioning of the built mechanical arm path, the measurement accuracy of the pose tracking device is higher, so that the clamp coordinate system is relative to the measurement coordinate system
And the tool coordinate system relative to the measurement coordinate system
The pose relationship is accurate; the workpiece coordinate system is relative to the fixture coordinate system
The pose relationship is reliable because the geometric dimension of the clamp is determined; ensuring dimensional accuracy by machining, the TCP tool tip coordinate system being relative to the tool coordinate system
The pose relationship of (2) is also reliable.
However, in this system, the nominal D-H parameters of the robot arm deviate significantly from the actual D-H parameters due to the robot arm's influence from machining errors, assembly errors, component wear, end load variations, and temperature. Based on the nominal D-H parameter, the positive kinematics of the robot arm can be expressed as
Wherein g represents a positive kinematic equation,
representing the nominal D-H parameter, the superscript m indicating the number of links and the subscript n indicating the nominal value. The error between the actual D-H parameter and the nominal D-H parameter can be expressed as the following relationship:
wherein [ Delta alpha ]TΔaT，ΔdT，ΔθT]TThe error of the D-H parameter is indicated and the subscript a indicates the actual value. Therefore, the actual pose of the robot arm end coordinate system relative to the frame coordinate system can be expressed as
Meanwhile, because the tool has installation error, the following relationship exists between the actual coordinate system of the tool relative to the tail end coordinate of the mechanical arm and the nominal coordinate system of the tool relative to the tail end coordinate of the mechanical arm
The pose relationship of the nominal machine base coordinate system relative to the measurement coordinate system can be obtained according to the formula (4), and similarly, the following relationship exists between the actual machine base coordinate system relative to the measurement coordinate system and the nominal machine base coordinate system relative to the measurement coordinate system
By introducing the error of each part into the system according to the above relationship, the following relationship can be obtained
From the above equation (13), the pose relationship of the TCP tool end coordinate system with respect to the measurement coordinate system is mainly affected by the pose of the machine base coordinate system with respect to the measurement coordinate system, the robot arm D-H parameters, and the pose of the tool coordinate system with respect to the robot arm end coordinate system. Therefore, equation (13) can be converted into the following kinematic model of the system
yMC＝f(yMB，αT，aT，dT，θT，yET) (14)
Wherein y isMC＝[xMC，yMC，zMC，φMC，βMC，γMC]TPose of TCP tool end coordinate system relative to measurement coordinate system expressed as vector, [ x ]MC，yMC，zMC]TRepresents a position vector, [ phi ]MC，βMC，γMC]TRepresenting an attitude vector, represented by an Euler angle; y isMBAnd yETAnd is also a pose vector. Generally, the tail end of a TCP tool is required to move along an expected track, and based on a pose tracking device, the pose of the expected track based on a measurement system can be obtained, so that the inverse solution f of the kinematics of the system can be obtained-1Obtaining a mechanical arm motion joint angle theta corresponding to motion along an expected trackT。
Step 104: and obtaining a mechanical arm motion joint angle corresponding to the motion along the expected track according to the inverse solution of the kinematic model.
Step 105: and compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path.
After the kinematics model is established, due to errors, the mechanical arm cannot accurately realize accurate positioning of the path, so the method uses an iterative learning control method, achieves the aim of accurate positioning of the path of the mechanical arm through iterative correction, can realize convergence within given time, does not depend on an accurate mathematic model, and can well accord with a final target based on the characteristics of the points. The method is applied to the accurate positioning of the path of the mechanical arm, and the specific flow is as follows: based on the pose tracking device, the pose of the expected track based on the measurement system can be determined according to the inverse solution f of the system kinematics-1The method comprises the following steps of calculating a motion joint angle of the mechanical arm corresponding to motion along an expected track, measuring positioning deviation of the mechanical arm through a pose tracking device in the motion process, inputting the positioning deviation into a controller, outputting joint compensation quantity of a current point on a path, putting the value into an iteration memory, and defining the path tracking and positioning completed by the mechanical arm for the first time as first iteration; and then, carrying out second iteration, carrying out pose error compensation based on the first joint compensation amount, regenerating a path, measuring the positioning deviation by using a pose tracking device again, inputting the positioning deviation into the controller, outputting the joint compensation amount when the path is positioned for the second time, putting the value into an iteration memory, repeating the steps until the pose error of each point position on the whole path is less than the set error, stopping the iteration process, defining the motion joint angle of the mechanical arm generated by the last iteration as a final motion output form, and processing the workpiece. In the iterative process, the positioning error caused by other parameters of the system is compensated by compensating the joint angle of the mechanical arm movement. When the path is accurately positioned, usually one iteration is difficult to realize higher positioning accuracy, and a larger positioning error still exists, so that repeated iteration is needed for many times, the positioning error is gradually reduced, and finally higher path accuracy is realized.
Due to the adoption of the scheme, the invention has the following advantages: in the invention, the method has stronger universality, does not limit the type and the number of the degrees of freedom of the mechanical arm, and can be generally applied to the field of manufacturing and processing of the mechanical arm; the position and posture tracking device is introduced, and the position and posture of the marker in the space can be measured to indirectly obtain the position and posture information of the research target in the space, so that the two markers are respectively arranged on the tool and the positioning clamp provided with the workpiece, and the position and posture relation between the workpiece and the mechanical arm can be preliminarily determined through the transformation of a coordinate system; the position tracking device can measure the positioning error of the terminal position of the TCP tool relative to a measurement coordinate system, and the iterative learning control method can effectively improve the positioning precision of the path of the mechanical arm, get rid of the defects of the conventional mechanical arm teaching and offline programming, make up the defects of the traditional control method and improve the automation level.
The specific embodiment is as follows:
and step 110, arranging a mechanical arm path tracking scene.
In this physical case, a complex curve in space is machined and positioned by a UR10 mechanical arm, a workpiece to be machined is composed of a circular surface with a radius of 400.00mm and an elliptical surface with a major axis of 700.00mm and a minor axis of 400.00mm, a desired path is defined in the middle of the width of the curve, namely a neutral plane, and simultaneously, the tip of the planning tool moves along the normal direction of the curve and has a height of 50.00 mm.
And 120, preliminarily determining the pose relation of the workpiece relative to the mechanical arm in the working environment.
In the embodiment of the scheme, based on the effectiveness of the UR10 mechanical arm description method, the position type of the mechanical arm is set to be
θ＝[-87.21，-79.56，127.15，-133.86，-90.98，-85.66]
Where the angle is in degrees. Based on UR10 nominal D-H parameter, the nominal pose of the mechanical arm end coordinate system relative to the base coordinate system can be obtained according to the positive kinematics of the mechanical arm
[xBE，n，yBE，n，zBE，n，φBE，n，βBE，n，γBE，n]
[ -191.63, 597.39, 209.01, 176.32, 1.16, -88.44], wherein x, y, z are in mm, and α, β, γ are in degrees. Meanwhile, the tool coordinate system can be measured by the pose tracking system relative to the measurement coordinate system as follows:
[xMT，yMT，zMt，φMT，βMT，γMT]＝[-591.84，-79.20，-3712.71，-178.98，0.00，175.36]
based on the CAD model, the nominal pose of the tool coordinate system relative to the mechanical arm tail end coordinate system is [ x ]ET，n，yET，n，zET，n，φET，n，βET，n，γET，n]＝[0.00，-98.50，85.00，-90.00，0.00，0.00]. Through the theory, the nominal pose of the machine base coordinate system relative to the measurement coordinate system can be obtained as follows:
[xMB，n，yMB，n，zMB，n，φMB，n，βMB，n，γMB，n]＝[-2.87，-197.58，-4013.59，-91.24，-0.99，91.46]
the position and posture of the TCP tool end coordinate system relative to the tool coordinate system are [ x ]TC，yTC，zTC，φTC，βTC，γTC]＝[0.00，-145.00，98.50，0.00，0.00，0.00]. The pose of the workpiece coordinate system relative to the fixture coordinate system is: [ x ] ofFW，yFW，zFW，φFW，βFW，γFW]＝[0.00，-260.00，135.50，0.00，0.00，0.00]. Meanwhile, the pose tracking system can measure the pose of the fixture coordinate system relative to the measurement coordinate system as xMF，yMF，zMF，φMF，βMF，γMF]＝[-159.41，-482.69，-3016.76，178.98，0.57，178.70]
Thus, the pose of the workpiece coordinate system with respect to the measurement coordinate system can be found to be [ x ]MW，yMW，zMW，φMW，βMW，γMW]＝[-152.18，-744.95，-3145.55，178.98，0.57，178.70]
Based on the data, the pose relationship between the workpiece and the mechanical arm in the working environment can be preliminarily determined as follows:
[xBW，n，yBW，n，zBW，n，φBW，n，βBW，n，γBW，n]＝[-851.65，180.81，-563.35，84.01，87.96，5.69]
meanwhile, the path line to be processed can be converted into a machine base coordinate system.
And step 130, establishing a system kinematic model.
Through step 120, the relationship between the coordinate systems can be preliminarily determined to plan the motion trajectory, but the deviation between the final actual motion trajectory of the mechanical arm and the expected trajectory is large. Establishing a system kinematic model according to the formula (14), and analyzing the system kinematic model mainly and yMB，αT，aT，dT，θT，yETParameters are related, but for a mechanical arm, only the joint angle theta of the mechanical arm can be controlledTTherefore, a pose error compensation mode is provided, the position error caused by other parameters can be compensated by adjusting the joint angle of the mechanical arm, and the positioning accuracy of the mechanical arm path is improved based on the method.
And 140, accurately positioning the path of the mechanical arm based on the pose tracking system by using an iterative learning control method.
In the control method, the method is mainly divided into three parts: a control object, an iterative memory, and a controller. In the invention, the control object is a mechanical arm, the control memory is mainly used for storing data of the last iteration, the controller is mainly used for converting the current positioning deviation into an input value of the control object, and the controller is designed to be
Where the subscript k denotes the number of iterations,. epsilon.is the system constant (0 < ε < 1), ei，kRepresenting the position of the end of the TCP tool relative to the measured coordinate system position between the measured value and the reference value when the k iteration moves to the i target pointError of (C)i，kIs a 6 Xm Jacobian matrix, which can be defined as the above kinematic model
The control system can realize accurate positioning of the mechanical arm path within limited iteration times. In this case, the desired path is planned, and a total of 80 target point locations are divided. In the motion process of the mechanical arm, the pose of the tail end of the TCP tool can be obtained through measurement of a pose tracking system, the actual pose of the tool at the tail end of the mechanical arm is compared with the defined pose of a planned path, the positioning error is converted into the error of a joint angle through a controller and is stored in an iteration memory, accurate positioning of the path is finally achieved through repeated iteration control, the process is iterated for 12 times in total, and the iteration result refers to FIGS. 4-9.
The invention also provides a pose tracking-based mechanical arm path generation system, which comprises:
the scene building module is used for building a mechanical arm path tracking scene;
the pose relation determining module is used for determining the pose relation between the workpiece and the mechanical arm in the working environment based on the mechanical arm path tracking scene;
the kinematic model establishing module is used for establishing a kinematic model based on the pose relation between the workpiece and the mechanical arm;
the mechanical arm motion joint angle determining module is used for solving a mechanical arm motion joint angle corresponding to motion along an expected track according to the inverse solution of the kinematic model;
and the compensation module is used for compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path.
Wherein, the pose relation determining module specifically comprises:
the first pose relation determining unit is used for obtaining pose relations of the tool coordinate system and the clamp coordinate system relative to the measurement coordinate system through the pose tracking device;
the second pose relation determining unit is used for determining the pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system according to the positive kinematics of the mechanical arm;
the third pose relation determining unit is used for determining the pose relation of a tool coordinate system relative to the tail end coordinate system of the mechanical arm and the pose relation of the TCP tool tail end coordinate system relative to the tool coordinate system based on the geometric dimension of the tool;
the fourth pose relation determining unit is used for determining the pose relation of the workpiece coordinate system relative to the clamp coordinate system according to the geometric dimension of the clamp;
and the fifth pose relation determination unit is used for determining the pose relation of the workpiece coordinate system relative to the stand coordinate system based on the pose relation of the tool coordinate system relative to the measurement coordinate system, the pose relation of the clamp coordinate system relative to the measurement coordinate system, the pose relation of the tail end coordinate system of the mechanical arm relative to the stand coordinate system, the pose relation of the tool coordinate system relative to the tail end coordinate system of the mechanical arm, the pose relation of the tail end coordinate system of the TCP tool relative to the tool coordinate system and the pose relation of the workpiece coordinate system relative to the clamp coordinate system.
The kinematic model building module specifically comprises:
the first actual pose relation determining unit is used for determining the actual pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system;
the second actual pose relation determining unit is used for determining the actual pose relation of the tool coordinate system relative to the terminal coordinate of the mechanical arm;
the third actual pose relation determining unit is used for determining the actual pose relation of the machine base coordinate system relative to the measurement coordinate system based on the actual pose relation of the mechanical arm tail end coordinate system relative to the machine base coordinate system and the actual pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system;
and the kinematic model establishing unit is used for proposing a kinematic model based on the actual pose relationship of the mechanical arm tail end coordinate system relative to the machine base coordinate system, the actual pose relationship of the tool coordinate system relative to the mechanical arm tail end coordinate system, the actual pose relationship of the machine base coordinate system relative to the measurement coordinate system and the pose relationship of the TCP tool tail end coordinate system relative to the tool coordinate system.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principle and the embodiment of the present invention are explained by applying specific examples, and the above description of the embodiments is only used to help understanding the method and the core idea of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the foregoing, the description is not to be taken in a limiting sense.
Claims (8)
1. A mechanical arm path generation method based on pose tracking is characterized by comprising the following steps:
constructing a mechanical arm path tracking scene;
determining a pose relationship between a workpiece and the mechanical arm in a working environment based on the path tracking scene of the mechanical arm;
establishing a kinematic model based on the pose relationship between the workpiece and the mechanical arm;
obtaining a mechanical arm motion joint angle corresponding to the motion along the expected track according to the inverse solution of the kinematic model;
and compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path.
2. The pose tracking-based mechanical arm path generation method according to claim 1, wherein the building of a mechanical arm path tracking scene specifically comprises:
fixing the mechanical arm provided with the cutter on a processing platform;
moving the pose tracking device to the right front of the mechanical arm;
and moving the mobile platform to the machining range of the mechanical arm, and simultaneously installing two markers on the clamp and the cutter respectively.
3. The pose tracking-based mechanical arm path generation method according to claim 2, wherein determining the pose relationship between the workpiece and the mechanical arm in the working environment based on the mechanical arm path tracking scene specifically comprises:
acquiring the pose relations of a tool coordinate system and a clamp coordinate system relative to a measurement coordinate system through a pose tracking device;
determining the pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system according to the positive kinematics of the mechanical arm;
determining the pose relationship of a tool coordinate system relative to a mechanical arm tail end coordinate system and the pose relationship of a TCP tool tail end coordinate system relative to the tool coordinate system based on the geometric dimension of the tool;
determining the pose relation of a workpiece coordinate system relative to a fixture coordinate system according to the geometric dimension of the fixture;
and determining the pose relation of the workpiece coordinate system relative to the stand coordinate system based on the pose relation of the tool coordinate system relative to the measuring coordinate system, the pose relation of the clamp coordinate system relative to the measuring coordinate system, the pose relation of the mechanical arm tail end coordinate system relative to the stand coordinate system, the pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system, the pose relation of the TCP tool tail end coordinate system relative to the tool coordinate system and the pose relation of the workpiece coordinate system relative to the clamp coordinate system.
4. The pose tracking-based mechanical arm path generation method according to claim 3, wherein the establishing of the kinematic model based on the pose relationship between the workpiece and the mechanical arm specifically comprises:
determining the actual pose relationship of the tail end coordinate system of the mechanical arm relative to the base coordinate system;
determining the actual pose relation of a tool coordinate system relative to the terminal coordinate of the mechanical arm;
determining the actual pose relation of the machine base coordinate system relative to the measurement coordinate system based on the actual pose relation of the mechanical arm tail end coordinate system relative to the machine base coordinate system and the actual pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system;
and suggesting a kinematics model based on the actual pose relationship of the mechanical arm tail end coordinate system relative to the machine base coordinate system, the actual pose relationship of the tool coordinate system relative to the mechanical arm tail end coordinate system, the actual pose relationship of the machine base coordinate system relative to the measurement coordinate system and the pose relationship of the TCP tool tail end coordinate system relative to the tool coordinate system.
5. The pose tracking-based mechanical arm path generation method according to claim 1, characterized in that an iterative learning control method is adopted to compensate the motion joint angle of the mechanical arm, and specifically comprises measuring a positioning deviation through a pose tracking device in the motion process, inputting the positioning deviation into a controller, outputting the joint compensation amount of the current point on the path, putting the joint compensation amount of the current point in an iteration memory, and defining the first time of completion of the tracking and positioning of the mechanical arm on the path as a first iteration; and then, carrying out second iteration, carrying out pose error compensation based on the first joint compensation amount, regenerating a path, measuring the positioning deviation by using a pose tracking device again, inputting the positioning deviation into the controller, outputting the joint compensation amount when the path is positioned for the second time, placing the joint compensation amount in an iteration memory, repeating the steps until the pose error of each point position on the whole path is smaller than the set error, stopping the iteration process, defining the motion joint angle of the mechanical arm generated by the last iteration as a final motion output form, and processing the workpiece.
6. A robotic arm path generation system based on pose tracking, comprising:
the scene building module is used for building a mechanical arm path tracking scene;
the pose relation determining module is used for determining the pose relation between the workpiece and the mechanical arm in the working environment based on the mechanical arm path tracking scene;
the kinematic model establishing module is used for establishing a kinematic model based on the pose relation between the workpiece and the mechanical arm;
the mechanical arm motion joint angle determining module is used for solving a mechanical arm motion joint angle corresponding to motion along an expected track according to the inverse solution of the kinematic model;
and the compensation module is used for compensating the motion joint angle of the mechanical arm by adopting an iterative learning control method to realize the positioning of the path.
7. The pose tracking-based robotic arm path generation system of claim 6, wherein the pose relationship determination module comprises:
the first pose relation determining unit is used for obtaining pose relations of the tool coordinate system and the clamp coordinate system relative to the measurement coordinate system through the pose tracking device;
the second pose relation determining unit is used for determining the pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system according to the positive kinematics of the mechanical arm;
the third pose relation determining unit is used for determining the pose relation of a tool coordinate system relative to the tail end coordinate system of the mechanical arm and the pose relation of a TCP tool tail end coordinate system relative to the tool coordinate system based on the geometric dimension of the tool;
the fourth pose relation determining unit is used for determining the pose relation of the workpiece coordinate system relative to the clamp coordinate system according to the geometric dimension of the clamp;
and the fifth pose relation determination unit is used for determining the pose relation of the workpiece coordinate system relative to the stand coordinate system based on the pose relation of the tool coordinate system relative to the measurement coordinate system, the pose relation of the clamp coordinate system relative to the measurement coordinate system, the pose relation of the tail end coordinate system of the mechanical arm relative to the stand coordinate system, the pose relation of the tool coordinate system relative to the tail end coordinate system of the mechanical arm, the pose relation of the tail end coordinate system of the TCP tool relative to the tool coordinate system and the pose relation of the workpiece coordinate system relative to the clamp coordinate system.
8. The pose tracking-based mechanical arm path generation method according to claim 7, wherein the kinematic model building module specifically comprises:
the first actual pose relation determining unit is used for determining the actual pose relation of the tail end coordinate system of the mechanical arm relative to the base coordinate system;
the second actual pose relation determining unit is used for determining the actual pose relation of the tool coordinate system relative to the terminal coordinate of the mechanical arm;
the third actual pose relation determining unit is used for determining the actual pose relation of the machine base coordinate system relative to the measurement coordinate system based on the actual pose relation of the mechanical arm tail end coordinate system relative to the machine base coordinate system and the actual pose relation of the tool coordinate system relative to the mechanical arm tail end coordinate system;
and the kinematic model establishing unit is used for proposing a kinematic model based on the actual pose relationship of the tail end coordinate system of the mechanical arm relative to the base coordinate system, the actual pose relationship of the tool coordinate system relative to the tail end coordinate of the mechanical arm, the actual pose relationship of the base coordinate system relative to the measuring coordinate system and the pose relationship of the tail end coordinate system of the TCP tool relative to the tool coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210250765.5A CN114505865A (en) | 2022-03-15 | 2022-03-15 | Pose tracking-based mechanical arm path generation method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210250765.5A CN114505865A (en) | 2022-03-15 | 2022-03-15 | Pose tracking-based mechanical arm path generation method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114505865A true CN114505865A (en) | 2022-05-17 |
Family
ID=81553273
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210250765.5A Pending CN114505865A (en) | 2022-03-15 | 2022-03-15 | Pose tracking-based mechanical arm path generation method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114505865A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115305979A (en) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | Method, processor and device for controlling engineering equipment arm support and engineering equipment |
| CN116147527A (en) * | 2023-02-15 | 2023-05-23 | 思看科技(杭州)股份有限公司 | Three-dimensional scanning system and scanning path planning method thereof |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150347846A1 (en) * | 2014-06-02 | 2015-12-03 | Microsoft Corporation | Tracking using sensor data |
| CN109623812A (en) * | 2018-12-04 | 2019-04-16 | 北京理工大学 | Consider the mechanical arm method for planning track of spacecraft ontology attitude motion |
| EP3581340A1 (en) * | 2018-06-13 | 2019-12-18 | Siemens Healthcare GmbH | Method for actuating a robot, corresponding data storage and robot |
| CN110948482A (en) * | 2019-11-06 | 2020-04-03 | 江苏信息职业技术学院 | Redundant robot trajectory planning method |
| CN112936273A (en) * | 2021-02-04 | 2021-06-11 | 清华大学深圳国际研究生院 | Speed-level kinematics modeling method of rope-driven flexible mechanical arm |
| CN113561183A (en) * | 2021-07-26 | 2021-10-29 | 上海大学 | Pose tracking system based mechanical arm target point online tracking method and system |
-
2022
- 2022-03-15 CN CN202210250765.5A patent/CN114505865A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150347846A1 (en) * | 2014-06-02 | 2015-12-03 | Microsoft Corporation | Tracking using sensor data |
| EP3581340A1 (en) * | 2018-06-13 | 2019-12-18 | Siemens Healthcare GmbH | Method for actuating a robot, corresponding data storage and robot |
| CN109623812A (en) * | 2018-12-04 | 2019-04-16 | 北京理工大学 | Consider the mechanical arm method for planning track of spacecraft ontology attitude motion |
| CN110948482A (en) * | 2019-11-06 | 2020-04-03 | 江苏信息职业技术学院 | Redundant robot trajectory planning method |
| CN112936273A (en) * | 2021-02-04 | 2021-06-11 | 清华大学深圳国际研究生院 | Speed-level kinematics modeling method of rope-driven flexible mechanical arm |
| CN113561183A (en) * | 2021-07-26 | 2021-10-29 | 上海大学 | Pose tracking system based mechanical arm target point online tracking method and system |
Non-Patent Citations (2)
| Title |
|---|
| SELEEM, IA等: "Development and Stability Analysis of an Imitation Learning-Based Pose Planning Approach for Multi-Section Continuum Robot", 《JOURNAL CITATION REPORTS》 * |
| 陈思安;葛为民;王肖锋;刘增昌;刘军: "新型可重构机器人的模糊自适应控制研究", 《控制工程》, pages 2399 - 2404 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115305979A (en) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | Method, processor and device for controlling engineering equipment arm support and engineering equipment |
| CN115305979B (en) * | 2022-08-16 | 2023-09-19 | 湖南中联重科智能高空作业机械有限公司 | Method, processor, device and engineering equipment for controlling arm support of engineering equipment |
| CN116147527A (en) * | 2023-02-15 | 2023-05-23 | 思看科技(杭州)股份有限公司 | Three-dimensional scanning system and scanning path planning method thereof |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110193829B (en) | Robot precision control method for coupling kinematics and rigidity parameter identification | |
| CN108748159B (en) | Self-calibration method for tool coordinate system of mechanical arm | |
| CN111367236B (en) | Mobile robot system calibration method and system for machining process | |
| CN108908327B (en) | Robot positioning error grading compensation method | |
| Wu et al. | Geometric calibration of industrial robots using enhanced partial pose measurements and design of experiments | |
| CN106777656B (en) | Industrial robot absolute accuracy calibration method based on PMPSD | |
| US4831549A (en) | Device and method for correction of robot inaccuracy | |
| CN113001535B (en) | Automatic correction system and method for robot workpiece coordinate system | |
| Fu et al. | Product-of-exponential formulas for precision enhancement of five-axis machine tools via geometric error modeling and compensation | |
| CN114505865A (en) | Pose tracking-based mechanical arm path generation method and system | |
| CN111300406B (en) | Industrial robot track precision compensation system and method based on kinematic analysis | |
| CN110948504B (en) | Normal constant force tracking method and device for robot machining operation | |
| CN109366220B (en) | Workpiece positioning method and system | |
| CN102458779A (en) | Robot calibration apparatus and method for same | |
| WO2010060459A1 (en) | A method and an apparatus for calibration of an industrial robot system | |
| CN109176517B (en) | Series industrial robot connecting rod parameter calibration method based on terminal nominal point constraint | |
| CN107457785B (en) | Robot position compensation method based on joint feedback | |
| CN114012724B (en) | Automatic calibration method for industrial robot coordinate system based on probe | |
| CN112894814B (en) | Mechanical arm DH parameter identification method based on least square method | |
| CN112902898B (en) | Three-dimensional measuring device and applicable mechanical arm correction method | |
| Leali et al. | A calibration method for the integrated design of finishing robotic workcells in the aerospace industry | |
| CN109397293B (en) | Ground level error modeling and compensating method based on mobile robot | |
| US20230191611A1 (en) | Robot system | |
| CN114800529A (en) | Industrial robot positioning error online compensation method based on fixed-length memory window incremental learning and incremental model reconstruction | |
| CN209840976U (en) | Measuring device for compensating thermal deformation error of industrial robot |
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 |