阅读说明：本技术 一种基于位姿跟踪系统机械臂目标点在线追踪方法及系统 (Pose tracking system based mechanical arm target point online tracking method and system ) 是由 李育文 刘颖 刘碧珊 王昶茹 于 2021-09-17 设计创作，主要内容包括：本发明涉及一种基于位姿跟踪系统机械臂目标点在线追踪方法及系统。该方法包括：确定机械臂加工场景；在机械臂加工场景中,利用所述位姿跟踪系统,确定待加工工件基于位姿追踪系统的位姿以及刀具基于位姿跟踪系统的位姿,根据几何关系可以初步确定刀具基于所述机械臂末端的位姿以及刀具末端相对于刀具的位姿,通过系统坐标变化关系,可以初步确定机械臂机座相对于位姿跟踪系统的位姿；根据所述待加工工件基于光学追踪系统的位姿、机械臂机座相对于位姿跟踪系统的位姿、刀具基于所述机械臂末端的位姿以及刀具末端相对于刀具的位姿构建系统运行学模型；根据所述系统运动学模型,利用位姿跟踪系统建立系统误差模型；利用所述系统误差模型对所述机械臂的目标点进行在线位姿误差补偿。本发明现了柔性化、高精度的加工制造,同时也有效提高了机械臂自动化水平。(The invention relates to a pose tracking system-based mechanical arm target point online tracking method and system. The method comprises the following steps: determining a mechanical arm processing scene; in a machining scene of the mechanical arm, the pose tracking system is utilized to determine the pose of a workpiece to be machined based on the pose tracking system and the pose of a tool based on the pose tracking system, the pose of the tool based on the tail end of the mechanical arm and the pose of the tail end of the tool relative to the tool can be preliminarily determined according to the geometric relationship, and the pose of a mechanical arm base relative to the pose tracking system can be preliminarily determined through the coordinate change relationship of the system; constructing a system operation model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool; establishing a system error model by using a pose tracking system according to the system kinematics model; and performing online pose error compensation on the target point of the mechanical arm by using the system error model. The invention realizes flexible and high-precision processing and manufacturing, and effectively improves the automation level of the mechanical arm.)

1. A pose tracking system based mechanical arm target point online tracking method is characterized by comprising the following steps:

determining a mechanical arm processing scene; the mechanical arm processing scene is that a workpiece to be processed and a rigid body are arranged on a clamp, a corresponding rigid body is arranged on a cutter at the tail end of a mechanical arm, and the mechanical arm is arranged on a processing platform; the pose tracking system is used for measuring the position and the posture of the rigid body in the space; the pose tracking system includes: an optical tracking system; the optical tracking system includes: two cameras;

in the mechanical arm processing scene, the pose tracking system is utilized to determine the pose of a workpiece to be processed based on the pose tracking system and the pose of a tool based on the pose tracking system, the pose of the tool based on the tail end of the mechanical arm and the pose of the tail end of the tool relative to the tool can be preliminarily determined according to the geometric relationship, and the pose of a mechanical arm base relative to the pose tracking system can be preliminarily determined through the system coordinate change relationship;

constructing a system operation model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool;

establishing a system error model by using a pose tracking system according to the system kinematics model;

and performing online pose error compensation on the target point of the mechanical arm by using the system error model.

2. The pose tracking system-based on-line tracking method for the target point of the mechanical arm is characterized in that in the mechanical arm processing scene, the pose tracking system is used for determining the pose of a workpiece to be processed based on the pose tracking system and the pose of a tool based on the pose tracking system; and determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship, wherein the pose tracking system specifically comprises the following steps:

determining the position and pose of the workpiece to be processed based on the position and pose tracking system and the position and pose of the cutter based on the position and pose tracking system according to the position and pose tracking system;

determining the pose of the tool based on the tail end of the mechanical arm according to the geometric relation between the tail end of the mechanical arm and the tool;

determining the pose of the tail end of the mechanical arm based on the mechanical arm base according to the current position of the mechanical arm and the positive kinematics solution of the mechanical arm;

using the formula T_MB＝T_MT(T_BET_ET)^-1Determining the pose of the mechanical arm base relative to a pose tracking system;

using the formula T_BW＝(T_MB)^-1T_MWDetermining the pose of a workpiece to be machined relative to a mechanical arm base;

determining the pose of the tool tail end relative to the tool according to the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the mechanical arm based on the mechanical arm base and the pose of the workpiece to be machined relative to the mechanical arm base; during machining, the pose of the tail end of the cutter is the same as the pose of the workpiece to be machined;

wherein, T_MBFor the pose, T, of the robot arm stand relative to the pose tracking system_MTBased on the position and orientation of the toolPose of tracking System, T_BEFor the pose, T, of the end of the arm based on the arm base_ETFor the pose of the tool on the basis of the end of the arm, T_BWFor the pose, T, of the workpiece to be machined relative to the robot arm stand_MWAnd the position and pose of the workpiece to be processed are based on a position and pose tracking system.

3. The pose tracking system-based on-line tracking method for the target point of the mechanical arm is characterized in that in the mechanical arm processing scene, the pose tracking system is used for determining the pose of a workpiece to be processed based on the pose tracking system and the pose of a tool based on the pose tracking system; and determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship, wherein the method also comprises the following steps:

establishing a measurement coordinate system in the pose tracking system; establishing a workpiece coordinate system by taking the rigid body arranged on the workpiece to be processed as an original point; establishing a cutter coordinate system by taking a rigid body mounted on a cutter as an original point; establishing a coordinate system of the tail end of the mechanical arm by taking the tail end of the mechanical arm as an original point; establishing a mechanical arm base coordinate system by taking the mechanical arm base as an original point; and establishing a tool tail end coordinate system by taking the tool tail end as an origin.

4. The method for tracking the target point of the mechanical arm on line based on the pose tracking system of the manipulator according to claim 2, wherein the method for constructing the system operation model according to the pose of the workpiece to be processed based on the optical tracking system, the pose of the mechanical arm base relative to the pose tracking system, the pose of the tool base on the tail end of the mechanical arm and the pose of the tool tail end relative to the tool comprises the following steps:

using the formula y_MC＝f(y_MB,y_ET,y_TC,g(p_n,x_r) Determining a system kinematics model;

wherein, y_MCAs pose vector of tool tip relative to tool, y_MBFor the robot arm stand relative to the pose tracking systemPosition vector of system, y_ETIs the pose vector y of the tool based on the tail end of the mechanical arm_TCFor the pose of the tool tip relative to the tool, f represents the positive kinematics of the system, g represents the positive kinematics of the robot arm, p_nRepresenting nominal parameters of the arm, x_rRepresenting the joint angle of the robotic arm.

5. The pose tracking system-based mechanical arm target point online tracking method according to claim 4, wherein the establishing of the system error model by using the pose tracking system according to the system kinematics model specifically comprises:

acquiring a joint angle value corresponding to the mechanical arm and a tool pose measured by a pose tracking system based on sampling frequency;

determining the pose of the tool end coordinate system output at the current moment relative to a pose tracking system according to the joint angle value at the current moment, the tool pose at the current moment measured by the pose tracking system and the kinematic model of the system;

comparing the pose of the tool tail end coordinate system output at the current moment relative to the pose tracking system with the actually measured pose at the current moment to determine the positioning error of the tool tail end at the current moment;

determining the system kinematic parameter error at the current moment according to the positioning error of the cutter tail end at the current moment; continuously and iteratively updating in the motion process of the mechanical arm, replacing the joint angle value at the current moment with the joint angle value at the next moment, replacing the tool pose at the current moment with the tool pose at the next moment, and returning to the step of determining the pose of the output tool end coordinate system at the current moment relative to the pose tracking system according to the joint angle value at the current moment, the tool pose at the current moment obtained by measurement of the pose tracking system and the system kinematic model until the target moment when the mechanical arm reaches the target point of the mechanical arm, and determining the positioning error of the tool end at the target moment and the system kinematic parameter error; the system kinematic parameter errors include: the method comprises the following steps of (1) parameter errors of the mechanical arm, installation errors of a cutter and base positioning errors of the mechanical arm;

according to the positioning error of the cutter end at the target moment and the system kinematic parameter error, utilizing a formula y_MC+Δy_MC＝f(y_MB+Δy_MB,y_ET+Δy_ET,y_TC,g(p_n+Δp_n,x_r) Determining a system error model;

wherein, Δ y_MCPositioning error of tool end at target moment, Δ p_nIs the parameter error, delta y, of the mechanical arm at the target moment_ETMounting error, Δ y, of the tool at the target time_MBThe machine base positioning error of the mechanical arm at the target moment.

6. The utility model provides a based on online tracker of position appearance tracker arm target point which characterized in that includes:

the mechanical arm processing scene determining module is used for determining a mechanical arm processing scene; the mechanical arm processing scene is that a workpiece to be processed and a rigid body are arranged on a clamp, a corresponding rigid body is arranged on a cutter at the tail end of a mechanical arm, and the mechanical arm is arranged on a processing platform; the pose tracking system is used for measuring the position and the posture of the rigid body in the space; the pose tracking system includes: an optical tracking system; the optical tracking system includes: two cameras;

the initial pose determining module is used for determining the pose of the workpiece to be processed based on the pose tracking system and the pose of the tool based on the pose tracking system in the mechanical arm processing scene by using the pose tracking system; determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship;

the system operation model building module is used for building a system operation model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool;

the system error model establishing module is used for establishing a system error model by utilizing a pose tracking system according to the system kinematics model;

and the pose error compensation module is used for performing online pose error compensation on the target point of the mechanical arm by using the system error model.

7. The pose tracking system-based manipulator target point online tracking system of claim 6, wherein the pose determination module comprises:

the tool position and posture determining unit is used for determining the position and posture of the workpiece to be machined based on the position and posture tracking system and the position and posture of the tool based on the position and posture tracking system according to the position and posture tracking system;

the tool pose determining unit is used for determining the pose of the tool based on the tail end of the mechanical arm according to the geometric relationship between the tail end of the mechanical arm and the tool;

the pose determining unit is used for determining the pose of the tail end of the mechanical arm based on the mechanical arm base according to the current position of the mechanical arm and the kinematics positive solution of the mechanical arm;

a pose determination unit of the mechanical arm base relative to the pose tracking system for utilizing a formula T_MB＝T_MT(T_BET_ET)^-1Determining the pose of the mechanical arm base relative to a pose tracking system;

a pose determining unit for determining the pose of the workpiece to be processed relative to the mechanical arm base by using a formula T_BW＝(T_MB)^-1T_MWDetermining the pose of a workpiece to be machined relative to a mechanical arm base;

the tool tail end relative to tool position determining unit is used for determining the tool tail end relative to tool position according to the tool position based on the mechanical arm tail end, the mechanical arm tail end based on the mechanical arm base and the workpiece to be machined position relative to the mechanical arm base; during machining, the pose of the tail end of the cutter is the same as the pose of the workpiece to be machined;

wherein, T_MBFor the pose, T, of the robot arm stand relative to the pose tracking system_MTFor tool based on pose tracking system, T_BEFor the pose, T, of the end of the arm based on the arm base_ETFor the pose of the tool on the basis of the end of the arm, T_BWFor the pose, T, of the workpiece to be machined relative to the robot arm stand_MWAnd the position and pose of the workpiece to be processed are based on a position and pose tracking system.

8. The pose tracking system based on manipulator target point online tracking system of claim 6, further comprising:

the coordinate system establishing module is used for establishing a measuring coordinate system in the pose tracking system; establishing a workpiece coordinate system by taking the rigid body arranged on the workpiece to be processed as an original point; establishing a cutter coordinate system by taking a rigid body mounted on a cutter as an original point; establishing a coordinate system of the tail end of the mechanical arm by taking the tail end of the mechanical arm as an original point; establishing a mechanical arm base coordinate system by taking the mechanical arm base as an original point; and establishing a tool tail end coordinate system by taking the tool tail end as an origin.

Technical Field

The invention relates to the field of workpiece processing, in particular to a pose tracking system-based method and a pose tracking system for tracking a target point of a mechanical arm on line.

Background

Based on the customized production mode, a flexible production mode with small batch, multiple varieties and high precision becomes the mainstream production mode of the manufacturing industry, and also puts higher requirements on the coming industry 4.0. The mechanical arm is used as a novel production tool for human, and the production mode of people is changed. Currently, two basic modes of actual operation of a robotic arm include: off-line programming and mechanical arm teaching. 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. Although the offline programming improves the automation level of the mechanical arm, the absolute positioning accuracy of the mechanical arm is low due to the influence of machining errors, assembly errors, component wear, end load variation and temperature, which adversely affects the wide application of the mechanical arm, so that the mechanical arm cannot reach a specified position point according to an ideal planned track.

In the machining process, not only the tool is required to be accurately positioned, but also the space pose of the workpiece to be machined needs to be accurately acquired. In order to accurately obtain the relative pose relationship between the mechanical arm and the workpiece, a plurality of points are usually taught by the mechanical arm to establish a workpiece coordinate system, the positioning precision of the method is low, and the processing efficiency is reduced; and a special clamp is also used for determining the relation between the workpiece and the fixture, and the matched fixture is designed according to different workpiece requirements.

Aiming at the problems, a mechanical arm manufacturing and processing method with high flexibility, high precision and high automation level needs to be established urgently, the positioning precision of the mechanical arm needs to be ensured, the production efficiency is improved to a certain degree, and meanwhile the pose of a workpiece in the space can be accurately obtained.

Disclosure of Invention

The invention aims to provide a method and a system for tracking a target point of a mechanical arm on line based on a pose tracking system, which realize flexible and high-precision machining and manufacturing and effectively improve the automation level of the mechanical arm.

In order to achieve the purpose, the invention provides the following scheme:

a pose tracking system based mechanical arm target point online tracking method comprises the following steps:

determining a mechanical arm processing scene; the mechanical arm processing scene is that a workpiece to be processed and a rigid body are arranged on a clamp, a corresponding rigid body is arranged on a cutter at the tail end of a mechanical arm, and the mechanical arm is arranged on a processing platform; the pose tracking system is used for measuring the position and the posture of the rigid body in the space; the pose tracking system includes: an optical tracking system; the optical tracking system includes: two cameras;

in the mechanical arm processing scene, determining the position and posture of the workpiece to be processed based on the position and posture tracking system and the position and posture of the tool based on the position and posture tracking system by using the position and posture tracking system; determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship;

constructing a system operation model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool;

establishing a system error model by using a pose tracking system according to the system kinematics model;

and performing online pose error compensation on the target point of the mechanical arm by using the system error model.

Optionally, in the mechanical arm machining scene, determining the pose of the workpiece to be machined based on the pose tracking system and the pose of the tool based on the pose tracking system by using the pose tracking system; and determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship, wherein the pose tracking system specifically comprises the following steps:

determining the position and pose of the workpiece to be processed based on the position and pose tracking system and the position and pose of the cutter based on the position and pose tracking system according to the position and pose tracking system;

determining the pose of the tool based on the tail end of the mechanical arm according to the geometric relation between the tail end of the mechanical arm and the tool;

determining the pose of the tail end of the mechanical arm based on the mechanical arm base according to the current position of the mechanical arm and the positive kinematics solution of the mechanical arm;

using the formula T_MB＝T_MT(T_BET_ET)^-1Determining the pose of the mechanical arm base relative to a pose tracking system;

using the formula T_BW＝(T_MB)^-1T_MWDetermining the pose of a workpiece to be machined relative to a mechanical arm base;

determining the pose of the tool tail end relative to the tool according to the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the mechanical arm based on the mechanical arm base and the pose of the workpiece to be machined relative to the mechanical arm base; during machining, the pose of the tail end of the cutter is the same as the pose of the workpiece to be machined;

wherein, T_MBFor the pose, T, of the robot arm stand relative to the pose tracking system_MTFor tool based on pose tracking system, T_BEFor the pose, T, of the end of the arm based on the arm base_ETFor the pose of the tool on the basis of the end of the arm, T_BWFor the pose, T, of the workpiece to be machined relative to the robot arm stand_MWAnd the position and pose of the workpiece to be processed are based on a position and pose tracking system.

Optionally, in the mechanical arm machining scene, determining the pose of the workpiece to be machined based on the pose tracking system and the pose of the tool based on the pose tracking system by using the pose tracking system; and determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship, wherein the method also comprises the following steps:

establishing a measurement coordinate system in the pose tracking system; establishing a workpiece coordinate system by taking the rigid body arranged on the workpiece to be processed as an original point; establishing a cutter coordinate system by taking a rigid body mounted on a cutter as an original point; establishing a coordinate system of the tail end of the mechanical arm by taking the tail end of the mechanical arm as an original point; establishing a mechanical arm base coordinate system by taking the mechanical arm base as an original point; and establishing a tool tail end coordinate system by taking the tool tail end as an origin.

Optionally, the constructing a system operational model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the end of the mechanical arm, and the position and posture of the tool end relative to the tool specifically includes:

using the formula y_MC＝f(y_MB,y_ET,y_TC,g(p_n,x_r) Determining a system kinematics model;

wherein, y_MCFor the pose vector, y, of the tool tip relative to the pose tracking system_MBPose vector, y, for the robot arm stand relative to the pose tracking system_ETIs the pose vector y of the tool based on the tail end of the mechanical arm_TCFor the pose of the tool tip relative to the tool, f represents the positive kinematics of the system, g represents the positive kinematics of the robot arm, p_nRepresenting nominal parameters of the arm, x_rRepresenting the joint angle of the robotic arm.

Optionally, the establishing a system error model by using a pose tracking system according to the system kinematics model specifically includes:

acquiring a joint angle value corresponding to the mechanical arm and a tool pose measured by a pose tracking system based on sampling frequency;

determining the pose of the tool end coordinate system output at the current moment relative to a pose tracking system according to the joint angle value at the current moment, the tool pose at the current moment measured by the pose tracking system and the kinematic model of the system;

comparing the pose of the tool tail end coordinate system output at the current moment relative to the pose tracking system with the actually measured pose at the current moment to determine the positioning error of the tool tail end at the current moment;

determining the system kinematic parameter error at the current moment according to the positioning error of the cutter tail end at the current moment; continuously and iteratively updating in the motion process of the mechanical arm, replacing the joint angle value at the current moment with the joint angle value at the next moment, replacing the tool pose at the current moment with the tool pose at the next moment, and returning to the step of determining the pose of the output tool end coordinate system at the current moment relative to the pose tracking system according to the joint angle value at the current moment, the tool pose at the current moment obtained by measurement of the pose tracking system and the system kinematic model until the target moment when the mechanical arm reaches the target point of the mechanical arm, and determining the positioning error of the tool end at the target moment and the system kinematic parameter error; the system kinematic parameter errors include: the method comprises the following steps of (1) parameter errors of the mechanical arm, installation errors of a cutter and base positioning errors of the mechanical arm;

according to the positioning error of the cutter end at the target moment and the system kinematic parameter error, utilizing a formula y_MC+Δy_MC＝f(y_MB+Δy_MB,y_ET+Δy_ET,y_TC,g(p_n+Δp_n,x_r) Determining a system error model;

wherein, Δ y_MCPositioning error of tool end at target moment, Δ p_nIs the parameter error, delta y, of the mechanical arm at the target moment_ETOf tools at the target momentMounting error, Δ y_MBThe machine base positioning error of the mechanical arm at the target moment.

A mechanical arm target point on-line tracking system based on a pose tracking system comprises:

the mechanical arm processing scene determining module is used for determining a mechanical arm processing scene; the mechanical arm processing scene is that a workpiece to be processed and a rigid body are arranged on a clamp, a corresponding rigid body is arranged on a cutter at the tail end of a mechanical arm, and the mechanical arm is arranged on a processing platform; the pose tracking system is used for measuring the position and the posture of the rigid body in the space; the pose tracking system includes: an optical tracking system; the optical tracking system includes: two cameras;

the initial pose determining module is used for determining the pose of the workpiece to be processed based on the pose tracking system and the pose of the tool based on the pose tracking system in the mechanical arm processing scene by using the pose tracking system; determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship;

the system operation model building module is used for building a system operation model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool;

the system error model establishing module is used for establishing a system error model by utilizing a pose tracking system according to the system kinematics model;

and the pose error compensation module is used for performing online pose error compensation on the target point of the mechanical arm by using the system error model.

Optionally, the pose determination module specifically includes:

the tool position and posture determining unit is used for determining the position and posture of the workpiece to be machined based on the position and posture tracking system and the position and posture of the tool based on the position and posture tracking system according to the position and posture tracking system;

the tool pose determining unit is used for determining the pose of the tool based on the tail end of the mechanical arm according to the geometric relationship between the tail end of the mechanical arm and the tool;

the pose determining unit is used for determining the pose of the tail end of the mechanical arm based on the mechanical arm base according to the current position of the mechanical arm and the kinematics positive solution of the mechanical arm;

a pose determination unit of the mechanical arm base relative to the pose tracking system for utilizing a formula T_MB＝T_MT(T_BET_ET)^-1Determining the pose of the mechanical arm base relative to a pose tracking system;

a pose determining unit for determining the pose of the workpiece to be processed relative to the mechanical arm base by using a formula T_BW＝(T_MB)^-1T_MWDetermining the pose of a workpiece to be machined relative to a mechanical arm base;

the tool tail end relative to tool position determining unit is used for determining the tool tail end relative to tool position according to the tool position based on the mechanical arm tail end, the mechanical arm tail end based on the mechanical arm base and the workpiece to be machined position relative to the mechanical arm base; during machining, the pose of the tail end of the cutter is the same as the pose of the workpiece to be machined;

wherein, T_MBFor the pose, T, of the robot arm stand relative to the pose tracking system_MTFor tool based on pose tracking system, T_BEFor the pose, T, of the end of the arm based on the arm base_ETFor the pose of the tool on the basis of the end of the arm, T_BWFor the pose, T, of the workpiece to be machined relative to the robot arm stand_MWAnd the position and pose of the workpiece to be processed are based on a position and pose tracking system.

Optionally, the method further comprises:

the coordinate system establishing module is used for establishing a measuring coordinate system in the pose tracking system; establishing a workpiece coordinate system by taking the rigid body arranged on the workpiece to be processed as an original point; establishing a cutter coordinate system by taking a rigid body mounted on a cutter as an original point; establishing a coordinate system of the tail end of the mechanical arm by taking the tail end of the mechanical arm as an original point; establishing a mechanical arm base coordinate system by taking the mechanical arm base as an original point; and establishing a tool tail end coordinate system by taking the tool tail end as an origin.

According to the specific embodiment provided by the invention, the invention discloses the following technical effects:

according to the method and the system for tracking the target point of the mechanical arm on line based on the pose tracking system, the specific pose of a workpiece in the space can be accurately obtained based on the pose tracking system; meanwhile, the pose of the tool at the tail end of the mechanical arm can be obtained by using a pose tracking system. The method can effectively avoid the situation that the pose relationship between the workpiece and the mechanical arm is determined by using a teaching mode, and the system can be applied to the manufacturing and processing of different workpieces, thereby realizing the processing mode of flexible production and manufacturing. Because the absolute positioning precision of the mechanical arm is low, the mechanical arm cannot accurately reach a target position, the invention provides a pose tracking system-based measurement error online estimation mechanical arm kinematic parameter error, a positioning error of a mechanical arm base and an installation error of a tail end tool. In conclusion, the robot arm target point tracking method based on the pose tracking system provided by the invention realizes flexible and high-precision machining and manufacturing, and effectively improves the automation level of the robot arm. Therefore, the invention has wide application field and application prospect.

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 schematic flow chart of a robot arm target point on-line tracking method based on a pose tracking system provided by the invention;

FIG. 2 is a schematic view of a robot arm processing scenario

FIG. 3 is a schematic diagram of a coordinate system in a robot arm processing scene;

4-11 are schematic diagrams of parameter errors estimated in real time by 50 points;

FIG. 12 is an illustration of the effect of compensating the front and rear axle hole assembly;

fig. 13 is a schematic structural view of a robot arm target point on-line tracking system based on a pose tracking system provided by the 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 method and a system for tracking a target point of a mechanical arm on line based on a pose tracking system, which realize flexible and high-precision machining and manufacturing and effectively improve the automation level of the mechanical arm.

In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.

Fig. 1 is a schematic flow chart of a method for tracking a target point of a robot arm on line based on a pose tracking system, as shown in fig. 1, the method for tracking the target point of the robot arm on line based on the pose tracking system includes:

s101, determining a mechanical arm machining scene, and as shown in figure 2; the mechanical arm processing scene is that a workpiece to be processed and a rigid body are arranged on a clamp, a corresponding rigid body is arranged on a cutter at the tail end of a mechanical arm, and the mechanical arm is arranged on a processing platform; the pose tracking system is used for measuring the position and the posture of the rigid body in the space; the pose tracking system includes: an optical tracking system; the optical tracking system includes: two cameras;

s102, in the mechanical arm processing scene, determining the position and posture of the workpiece to be processed based on the position and posture tracking system and the position and posture of the cutter based on the position and posture tracking system by using the position and posture tracking system; and determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to the pose tracking system according to the geometric relationship and the reference object change relationship.

S102, previously further comprising:

establishing a measurement coordinate system in the pose tracking system; establishing a workpiece coordinate system by taking the rigid body arranged on the workpiece to be processed as an original point; establishing a cutter coordinate system by taking a rigid body mounted on a cutter as an original point; establishing a coordinate system of the tail end of the mechanical arm by taking the tail end of the mechanical arm as an original point; establishing a mechanical arm base coordinate system by taking the mechanical arm base as an original point; and establishing a tool tail end coordinate system by taking the tool tail end as an origin.

S102 specifically comprises the following steps:

determining the position and pose of the workpiece to be processed based on the position and pose tracking system and the position and pose of the cutter based on the position and pose tracking system according to the position and pose tracking system;

determining the pose of the tool based on the tail end of the mechanical arm according to the geometric relation between the tail end of the mechanical arm and the tool;

determining the pose of the tail end of the mechanical arm based on the mechanical arm base according to the current position of the mechanical arm and the positive kinematics solution of the mechanical arm;

using the formula T_MB＝T_MT(T_BET_ET)^-1Determining the pose of the mechanical arm base relative to a pose tracking system;

using the formula T_BW＝(T_MB)^-1T_MWDetermining the pose of a workpiece to be machined relative to a mechanical arm base;

determining the pose of the tool tail end relative to the tool according to the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the mechanical arm based on the mechanical arm base and the pose of the workpiece to be machined relative to the mechanical arm base; during machining, the pose of the tail end of the cutter is the same as the pose of the workpiece to be machined; namely, the pose of the tail end of the cutter is coincided with the pose of the station to be processed of the workpiece.

Wherein, T_MBFor the pose, T, of the robot arm stand relative to the pose tracking system_MTFor tool based on pose tracking system, T_BEFor the pose, T, of the end of the arm based on the arm base_ETFor the pose of the tool on the basis of the end of the arm, T_BWFor the pose, T, of the workpiece to be machined relative to the robot arm stand_MWAnd the position and pose of the workpiece to be processed are based on a position and pose tracking system.

Namely, the pose tracking system can accurately acquire the pose T of the workpiece in the space_MWWherein { M } represents a measurement coordinate system, the pose tracking system mainly comprises two phases of mechanisms, the measurement coordinate system is established between the two phases of mechanisms, and { W } represents a workpiece coordinate system, because the workpiece is arranged on the clamp, and the clamp is provided with a corresponding rigid body, the pose relation between the workpiece and the clamp can be accurately obtained, and the pose of the rigid body relative to the measurement coordinate system can be accurately obtained through the pose tracking system, so that the pose of the workpiece coordinate system relative to the measurement coordinate system can be obtained; meanwhile, the corresponding rigid body is also arranged on the cutter, so that the position and attitude of the cutter in the space can be accurately obtained as T based on the position and attitude tracking system_MTWhere { T } represents the tool coordinate system. In the measuring range of the pose tracking system, the initial pose of a group of mechanical arms is randomly selected, and the relative pose relation T between the tool coordinate system and the measuring coordinate system can be obtained through the pose tracking system_MT(ii) a According to the geometric relation, the pose of the tail end tool based on the tail end of the mechanical arm can be obtained as T_ETWhere { E } represents the robot arm tip coordinate system; based on the current position of the mechanical arm, the pose T of the tail end of the mechanical arm based on the mechanical arm base can be obtained through the kinematics forward solution of the mechanical arm_BEWhere { B } represents the robot base coordinate system. The relationship and establishment of each coordinate system are shown in fig. 3.

S103, constructing a system operational model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool;

s103 specifically comprises the following steps:

using the formula y_MC＝f(y_MB,y_ET,y_TC,g(p_n,x_r) Determining a system kinematics model;

wherein, y_MCAs pose vector of tool tip relative to tool, y_MBPose vector, y, for the robot arm stand relative to the pose tracking system_ETIs the pose vector y of the tool based on the tail end of the mechanical arm_TCFor the pose of the tool tip relative to the tool, f represents the positive kinematics of the system, g represents the positive kinematics of the robot arm, p_nRepresenting nominal parameters of the arm, x_rRepresenting the joint angle of the robotic arm.

And S104, establishing a system error model by using a pose tracking system according to the system kinematics model.

S104 specifically comprises the following steps:

acquiring a joint angle value corresponding to the mechanical arm and a tool pose measured by a pose tracking system based on sampling frequency;

determining the pose of the tool end coordinate system output at the current moment relative to a pose tracking system according to the joint angle value at the current moment, the tool pose at the current moment measured by the pose tracking system and the kinematic model of the system;

comparing the pose of the tool tail end coordinate system output at the current moment relative to the pose tracking system with the actually measured pose at the current moment to determine the positioning error of the tool tail end at the current moment;

determining the system kinematic parameter error at the current moment according to the positioning error of the cutter tail end at the current moment; continuously and iteratively updating in the motion process of the mechanical arm, replacing the joint angle value at the current moment with the joint angle value at the next moment, replacing the tool pose at the current moment with the tool pose at the next moment, and returning to the step of determining the pose of the output tool end coordinate system at the current moment relative to the pose tracking system according to the joint angle value at the current moment, the tool pose at the current moment obtained by measurement of the pose tracking system and the system kinematic model until the target moment when the mechanical arm reaches the target point of the mechanical arm, and determining the positioning error of the tool end at the target moment and the system kinematic parameter error; the system kinematic parameter errors include: the method comprises the following steps of (1) parameter errors of the mechanical arm, installation errors of a cutter and base positioning errors of the mechanical arm;

according to the positioning error of the cutter end at the target moment and the system kinematic parameter error, utilizing a formula y_MC+Δy_MC＝f(y_MB+Δy_MB,y_ET+Δy_ET,y_TC,g(p_n+Δp_n,x_r) Determining a system error model;

wherein, Δ y_MCPositioning error of tool end at target moment, Δ p_nIs the parameter error, delta y, of the mechanical arm at the target moment_ETMounting error, Δ y, of the tool at the target time_MBThe machine base positioning error of the mechanical arm at the target moment.

The nominal D-H parameters of the mechanical arm have deviation due to the influence of machining errors, assembly errors and other factors on the mechanical arm, and the deviation value is defined as delta p_n(ii) a Although the geometric dimension of the cutter is relatively accurate, certain assembly errors exist in the assembly process, and the error value is defined as delta y in the patent_ET(ii) a Similarly, the mechanical arm base has a positioning error relative to the pose tracking system, and the error value is defined to be delta y in the patent_MB. Due to the existence of the errors, the cutter cannot accurately reach the target point position, and the positioning error of the tail end of the cutter is delta y_MC。

Based on the same moment, joint angle values corresponding to the mechanical arm and tool pose information obtained by the pose tracking system can be acquired, the current joint angle values are substituted into a system kinematic equation, the pose of a nominal tool end coordinate system relative to the pose tracking system can be solved, and the obtained nominal pose is compared with the actually measured pose, so that the current positioning error, namely delta yMC, can be obtained. The system kinematic parameter errors, i.e. [ Δ pn, Δ yET, Δ yMB ], are estimated online based on the current positioning error. Similarly, based on a certain sampling frequency, data acquisition is repeatedly carried out in this way, the kinematic parameter error of the system is estimated on line, and finally the accurate estimation of the system error model can be realized.

Through the pose tracking system, the pose of the corresponding station to be processed on the workpiece in the space can be accurately obtained, and path planning is carried out based on the current initial state of the tool at the tail end of the mechanical arm. For the path planning of the mechanical arm, the position and the attitude of a mechanical arm base coordinate system relative to a measurement system coordinate system and the position and the attitude of a tool coordinate system relative to a mechanical arm tail end coordinate system are initialized according to nominal D-H parameters of the mechanical arm, and the mechanical arm joint angle corresponding to each point in the path is solved according to the inverse kinematics of the system. And issuing the solved joint angle to a mechanical arm control system, and measuring the pose of a tool at the tail end of the mechanical arm through a pose tracking system in the motion process of the mechanical arm. Comparing the actual pose of the tool at the tail end of the mechanical arm with the defined planned path pose to obtain a pose error, namely delta y_MCOn-line estimation system kinematic parameter error, namely [ delta p ], based on measurement error of pose tracking system_n、Δy_ET、Δy_MB]Finally, the measurement error delta y is realized by estimating the system kinematic parameter error on line_MCGradually reduced to meet the precision requirement of processing and positioning.

And S105, performing online pose error compensation on the target point of the mechanical arm by using the system error model.

In the process that the mechanical arm moves in real time through path planning, the kinematic parameter error of the system can be estimated on line based on the measurement error of the pose tracking system, so that after the mechanical arm moves to a target point, the system inverse kinematics solution is carried out through the estimated kinematic parameter error of the system, the online pose error compensation is carried out, the higher positioning precision of the mechanical arm is realized, meanwhile, the kinematic parameter error of the system is estimated on line in the motion process of the mechanical arm, and after the mechanical arm reaches the target point, the rapid pose error compensation is realized based on the estimated parameter error, and the processing efficiency is improved.

The invention is described in further detail below with reference to the accompanying drawings.

Referring to fig. 2, in this embodiment, an axial hole assembly experiment is performed by a UR10 robot arm, rigid bodies are respectively mounted on a tool and a workpiece, the positions and postures of the two rigid bodies in space can be measured in real time by a pose tracking system, and the workpiece is mounted on a movable platform in order to show the effectiveness of the experiment.

Through the pose tracking system, the pose relationship of the workpiece relative to the pose tracking system can be accurately obtained as follows:

y_MW＝[x_MB，y_MB，z_MB，α_MB，β_MB，γ_MB]

＝[295.64，-537.85，-3440.09，17837，0.36，179.25]；

wherein x, y, z are in mm, and α, β, γ are in degrees. Meanwhile, under the initial state of the mechanical arm, the pose of the tail end of the current-position lower tool is obtained as follows:

y_MC＝[-194.98，-151.31，-3792.28，178，37，0.36，179.25]

the pose of the mechanical arm base coordinate system relative to the measurement coordinate system can be obtained through calculation as follows:

y_MB＝[-104.89，-357.92，-4241.71，-90.50，-1.08，-1.44]

a system error model is established, and because the UR10 mechanical arm is adopted in the entity case, 24 parameter errors of D-H are totally adopted; meanwhile, the installation error of the tool and the positioning error of the mechanical arm base exist, so that the error model parameters of the system are 26 in total.

And planning a path from the current pose of the mechanical arm to a target point, wherein 50 points are divided in the process. In the motion process of the mechanical arm, a pose tracking system is used for measuring the current pose of a tool, the actual pose of the tool at the tail end of the mechanical arm is compared with the defined planning path pose, based on the measurement error of the pose tracking system, the kinematic parameter error of the system is estimated on line by using an extended Kalman filtering algorithm, and the method specifically comprises the following steps of prediction updating and measurement updating:

and (3) prediction updating:

and (3) measurement updating:

wherein the prediction update phaseRepresenting the kinematic parameter error, k | k-1 representing the predicted value at time k, k-1| k-1 representing the estimated value at time k-1,representing an estimated covariance matrix; and in the measurement updating stage, H represents a measurement matrix, R represents a covariance matrix of Gaussian white noise, K represents Kalman gain, and z represents a pose error. Referring to FIGS. 4-11, the parameter error is estimated in real time by 50 points.

Due to the existence of system errors, the mechanical arm cannot accurately reach the target point. And performing system inverse kinematics solution through the estimated system kinematics parameter error, and performing online pose error compensation, wherein the table 1 is the joint angle corresponding to the mechanical arm before and after compensation of the target point, and the table 2 is the positioning error before and after compensation. FIG. 12 is a diagram illustrating the effect of compensating for the fit of the front and rear axle holes.

Table 1 mechanical arm joint angle before and after on-line pose error compensation

TABLE 2 positioning error before and after on-line pose error compensation

Due to the adoption of the scheme, the invention has the following advantages: the method has strong universality, does not limit the freedom degree type of the mechanical arm, and can be generally applied to the field of manufacturing and processing of the mechanical arm; a pose tracking system is introduced, and the pose information of a research target in the space can be indirectly obtained by measuring the position and the posture of a rigid body in the space; the method is characterized in that a workpiece is arranged on a positioning clamp, and a rigid body is arranged on the positioning clamp, so that the specific pose of the workpiece in the space can be obtained through a pose tracking system, and not only is the specific pose of the workpiece in the space obtained through the pose tracking system, but also the corresponding rigid body is arranged on a tool at the tail end of a mechanical arm, the pose relation between the workpiece and the mechanical arm can be preliminarily determined through the method, the pose relation between the workpiece and the mechanical arm can be avoided being determined through a teaching method, the system can be applied to the manufacturing and processing of different workpieces, and a processing mode of flexible production and manufacturing is realized; the estimation of kinematic parameter errors of the mechanical arm in the motion process is realized based on the measurement errors of the pose tracking system, and online pose error compensation is performed through the estimated parameters after the mechanical arm reaches a target point, so that the absolute positioning precision of the mechanical arm is improved, and the automation level is also improved.

Fig. 13 is a schematic structural view of a robot arm target point on-line tracking system based on a pose tracking system, as shown in fig. 13, the robot arm target point on-line tracking system based on the pose tracking system provided by the present invention includes:

a mechanical arm processing scene determining module 1301, configured to determine a mechanical arm processing scene; the mechanical arm processing scene is that a workpiece to be processed and a rigid body are arranged on a clamp, a corresponding rigid body is arranged on a cutter at the tail end of a mechanical arm, and the mechanical arm is arranged on a processing platform; the pose tracking system is used for measuring the position and the posture of the rigid body in the space; the pose tracking system includes: an optical tracking system; the optical tracking system includes: two cameras;

an initial pose determining module 1302, configured to determine, by using the pose tracking system, a pose of the workpiece to be processed based on the pose tracking system and a pose of the tool based on the pose tracking system in the robot arm processing scene; determining the pose of the tool based on the tail end of the mechanical arm, the pose of the tail end of the tool relative to the tool and the pose of the mechanical arm base relative to a pose tracking system according to the geometric relationship and the reference object change relationship;

the system operation model building module 1303 is used for building a system operation model according to the position and posture of the workpiece to be processed based on the optical tracking system, the position and posture of the mechanical arm base relative to the position and posture tracking system, the position and posture of the tool based on the tail end of the mechanical arm and the position and posture of the tool tail end relative to the tool;

a system error model building module 1304, configured to build a system error model by using a pose tracking system according to the system kinematics model;

and a pose error compensation module 1305, configured to perform online pose error compensation on the target point of the mechanical arm by using the system error model.

The pose determination module 1302 specifically includes:

the tool position and posture determining unit is used for determining the position and posture of the workpiece to be machined based on the position and posture tracking system and the position and posture of the tool based on the position and posture tracking system according to the position and posture tracking system;

the tool pose determining unit is used for determining the pose of the tool based on the tail end of the mechanical arm according to the geometric relationship between the tail end of the mechanical arm and the tool;

the pose determining unit is used for determining the pose of the tail end of the mechanical arm based on the mechanical arm base according to the current position of the mechanical arm and the kinematics positive solution of the mechanical arm;

a pose determination unit of the mechanical arm base relative to the pose tracking system for utilizing a formula T_MB＝T_MT(T_BET_ET)^-1Determining the pose of the mechanical arm base relative to a pose tracking system;

a pose determining unit for determining the pose of the workpiece to be processed relative to the mechanical arm base by using a formula T_BW＝(T_MB)^-1T_MWDetermining the pose of a workpiece to be machined relative to a mechanical arm base;

the tool tail end relative to tool position determining unit is used for determining the tool tail end relative to tool position according to the tool position based on the mechanical arm tail end, the mechanical arm tail end based on the mechanical arm base and the workpiece to be machined position relative to the mechanical arm base; during machining, the pose of the tail end of the cutter is the same as the pose of the workpiece to be machined;

wherein, T_MBFor the pose, T, of the robot arm stand relative to the pose tracking system_MTFor tool based on pose tracking system, T_BEFor the pose, T, of the end of the arm based on the arm base_ETFor the pose of the tool on the basis of the end of the arm, T_BWFor the pose, T, of the workpiece to be machined relative to the robot arm stand_MWAnd the position and pose of the workpiece to be processed are based on a position and pose tracking system.

The invention provides a pose tracking system based on online tracking of a target point of a mechanical arm, which further comprises:

the coordinate system establishing module is used for establishing a measuring coordinate system in the pose tracking system; establishing a workpiece coordinate system by taking the rigid body arranged on the workpiece to be processed as an original point; establishing a cutter coordinate system by taking a rigid body mounted on a cutter as an original point; establishing a coordinate system of the tail end of the mechanical arm by taking the tail end of the mechanical arm as an original point; establishing a mechanical arm base coordinate system by taking the mechanical arm base as an original point; and establishing a tool tail end coordinate system by taking the tool tail end as an origin.

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 principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept 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 above, the present disclosure should not be construed as limiting the invention.