Accurate load stress sensing method based on parallel robot joint force sensor

Abstract

A load accurate stress sensing method based on a parallel robot joint force sensor comprises the following steps that (1) a light tracker measures a reflection target ball installed on a ball socket, and the installation inclination angle of the force sensor is calibrated; 2) based on a six-dimensional force calculation model of a joint force sensor, carrying out self-calibration on a base installation inclination angle of the parallel attitude adjusting mechanism; (3) the attitude mechanism runs according to the excitation track, observation data are collected, and online identification of load kinetic parameters is completed; and 4) compensating the non-contact force caused by the load by the kinetic model so as to realize the stress sensing of the tail end load. The invention provides an effective force measurement resolving method for scenes (such as active compliance and man-machine cooperation) needing to be sensed by the stress of the robot, the method can be realized only by depending on a parallel robot joint three-dimensional force sensor, and a six-dimensional force sensor does not need to be additionally mounted, so that the force measurement cost is saved, and the working efficiency is improved. And the force measurement precision is obviously improved through inclination angle compensation and parameter identification.

Description

Technical Field

[0001] The present invention relates to robot control and sensing technology, in particular to a method for calculating the end load force based on a parallel robot joint force sensor, specifically to a method for accurately sensing and determining the load force based on a parallel robot joint force sensor. Background Art

[0002] Large key hole shaft structures such as engine rotors, helicopter rotor systems and large antenna bases usually use interference fit. The temperature difference method is one of the main methods to achieve interference fit. During the assembly process, the fitting clearance between parts is small.

[0003] With the advancement of robotics, automation and sensor technology, automated assembly has gradually become mainstream. However, the traditional "digital measurement + rigid positioning" method is difficult to meet the above high-precision assembly requirements. For this reason, active compliant assembly has received widespread attention. Active compliant assembly refers to the robot using force sensors to sense the contact force between the parts to be assembled based on position control, and completing the assembly through position correction through control algorithms. Therefore, accurately sensing and determining the contact force between parts is crucial to achieving active compliance. Summary of the invention

[0004] The purpose of the present invention is to address the problem that the non-contact force caused by the load will interfere with the force perception during the active and compliant assembly of aviation interference hole shafts. A method for accurately determining the load force perception based on a parallel robot joint force sensor is invented. By compensating for the sensor installation inclination angle and identifying the load dynamic parameters, non-contact force compensation is accurately performed to achieve external contact force perception during the assembly process.

[0005] The technical solution of the present invention is:

[0006] A method for accurately sensing and determining load force based on a parallel robot joint force sensor comprises the following steps:

[0007] S1: Use a laser tracker to measure the reflective target balls installed on the base and the socket respectively, fit and calculate the posture of the socket coordinate system relative to the base coordinate system, and complete the calibration of the force sensor installation inclination angle;

[0008] S2: Construct a six-dimensional force solution model based on joint force sensors, and self-calibrate the installation inclination angle of the attitude adjustment mechanism base through the force measurement data of the attitude adjustment mechanism in static state;

[0009] S3: Make the posture adjustment mechanism run according to the excitation trajectory, record the load motion data and the force value measured by the three-dimensional force sensor, and identify the load dynamic parameters online;

[0010] S4: Compensate for the non-contact force caused by the load based on the dynamic model to achieve accurate force sensing of the end load.

[0011] Furthermore, the coordinate system constructed in the step S1 of force sensor installation inclination calibration and the step S2 of base installation inclination calibration includes: a world coordinate system O w -X w Y w Z w , its Z-axis direction is parallel to and opposite to gravity, and the coordinate system of the attitude adjustment mechanism base is O b -X b Y b Z b , the measurement coordinate system O of each joint force sensor si -X si Y si Z si , where i = {1, 2, 3}, the end load coordinate system O t -X t Y t Z t .

[0012] The specific steps of step S1 force sensor installation inclination calibration are as follows:

[0013] S1.1: Install a reflective target ball on the base of the attitude adjustment mechanism and use a laser tracker to measure. The coordinates of the reflective target ball in the base coordinate system are: b P i =[ b x i b y i b z i ], where i = {1, 2, 3, 4}, and the coordinates measured by the laser tracker are l P i =[ l x i l y i l z i ].

[0014] S1.2: Install a reflective target ball in the ball socket connected to the force sensor and use a laser tracker to measure. The coordinates of the reflective target ball in the force sensor measurement coordinate system are: l S i =[ l x i l y i b z i ], where i = {1, 2, 3}, and the coordinates measured by the laser tracker are lP i =[ l x i l y i l z i ].

[0015] S1.3: Use SVD decomposition to calculate the attitude matrix of the force sensor relative to the base And the attitude matrix of the base relative to the world coordinate system According to the rigid body rotation principle, the sensor installation inclination matrix can be calculated by the following formula:

[0016]

[0017] S1.4: The original measurement value of the three-dimensional force sensor is s F i =[ s x i s y i s z i ] T , after the inclination compensation, the sensor measurement value in the base coordinate system is:

[0018]

[0019] The step S2 of self-calibrating the installation inclination angle of the attitude adjustment mechanism base comprises the following steps:

[0020] S2.1: The posture of the terminal coordinate system in the base coordinate system is The coordinates of the ball joint in the tool coordinate system are: t r i =[ t x i t y i t z i ] T According to the translation theorem of force, the resultant force and moment of the end load in the base coordinate system are:

[0021]

[0022] S2.2: Define the base inclination angle. First rotate the base around the Y axis of the world coordinate system by an angle θ, then rotate the base around the X axis of the world coordinate system by an angle Then the transformation matrix of the base coordinates relative to the world coordinate system is:

[0023]

[0024] S2.3: When the attitude adjustment mechanism is in a stationary state, the relationship between gravity and the sensor measurement value is as follows:

[0025]

[0026] in, w G=[00-mg] T represents gravity, m is the load mass, g represents the acceleration of gravity, and the magnitude of gravity is The base installation inclination angle can be calculated by the following formula:

[0027]

[0028] S2.4: After the base installation inclination compensation, the gravity in the base coordinate system is:

[0029]

[0030] The step S3 of load dynamics parameter identification includes the following steps:

[0031] S3.1: Let the attitude adjustment mechanism run in free space according to the excitation trajectory. At this time, the external torque on the end load is zero and the acceleration is Angular velocity t ω, angular acceleration Center of mass coordinates r m , the resultant torque measured by the force sensor in the tool coordinate system can be expressed as:

[0032] t M=[LN][x] (24)

[0033] in:

[0034]

[0035] x=[r x r y r z I xx I yy I zz I xy I xz I yz ] T (27)

[0036] S3.2: Equation (9) can be simplified to i =b i x, continuous data collection can be obtained:

[0037] A=Bx (28)

[0038] A=[a1 T a2 T…a n T ] T (29)

[0039] B=[b1 T b2 T ...b n T ] T (30)

[0040] Kinetic parameter identification based on the least squares principle:

[0041] x=(B T B) -1 B T A (31)

[0042] The method for realizing force sensing by non-contact force compensation in step S4 is as follows:

[0043] During the terminal motion, the external force and torque are t f and t τ e , establish the dynamic model of the end load in the base coordinate system to compensate for the non-contact force and realize the load force perception:

[0044]

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] 1. The present invention realizes force perception based on the parallel robot joint force sensor. The parallel posture adjustment mechanism has the advantages of large load, high positioning accuracy and fast dynamic response. Therefore, the present invention can be used in the active and compliant assembly scenarios of large aviation parts. There is no need to install additional six-dimensional force sensors, which reduces the force measurement cost.

[0047] 2. For the sensor installation inclination angle and the robot base installation inclination angle, the posture transformation matrix is ​​obtained by measurement and self-calibration respectively, and the original measurement value of the sensor is mapped back to the reference coordinate system, thereby improving the accuracy of the original measurement data.

[0048] 3. Through online identification of load dynamic parameters, the problem of inaccurate parameters obtained using CAD digital models is avoided, the accuracy of non-contact force compensation is greatly improved, and the error of load force is significantly reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 The figure is a schematic diagram of the overall process of the method of the present invention.

[0050] Figure 2 It is a schematic diagram of the system composition and coordinate system relationship of the present invention. DETAILED DESCRIPTION

[0051] The technical solutions in the embodiments of the present invention are described clearly and completely below so that those skilled in the art can better understand the advantages and features of the present invention, thereby making a clearer definition of the protection scope of the present invention. Obviously, the embodiments described in the present invention are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present invention.

[0052] The present invention will be further described below in conjunction with the accompanying drawings and implementation methods.

[0053] like Figure 1-2 shown.

[0054] A method for accurately sensing load force based on joint force sensors of parallel robots, such as Figure 1 As shown, the specific steps are as follows:

[0055] Step 1: Construct coordinate relationship: world coordinate system O w -X w Y w Z w , its Z-axis direction is parallel to and opposite to gravity, and the coordinate system of the attitude adjustment mechanism base is O b -X b Y b Z b , the measurement coordinate system O of each joint force sensor si -X si Y si Z si , where i = {1, 2, 3}, the end load coordinate system O t -X t Y t Z t .

[0056] The laser tracker was used to measure the reflective target ball installed on the base and the socket respectively. Figure 2 The number of reflective target balls is 3, and each reflective target ball has 4 reflective holes), the postures of the three ball-socket coordinate systems relative to the base coordinate system are fitted and calculated to complete the calibration of the installation inclination angle of the force sensor;

[0057] A reflective target ball is installed on the base of the attitude adjustment mechanism and a laser tracker is used for measurement. The coordinates of the reflective target ball (each reflective target ball has 4 target ball holes) in the base coordinate system are: b P i =[ b x i b y i b zi ], where i = {1, 2, 3, 4}, and the coordinates measured by the laser tracker are l P i =[ l x i l y i l z i ].

[0058] In conjunction with the force sensor ( Figure 2 The force sensor in the figure is 3) connected to the ball socket to install the reflective target ball, and the laser tracker is used for measurement. The coordinates of the reflective target ball in the force sensor measurement coordinate system are l S i =[ l x i l y i b z i ], where i = {1, 2, 3}, and the coordinates measured by the laser tracker are l P i =[ l x i l y i l z i ].

[0059] Use SVD decomposition to calculate the posture matrix of the force sensor relative to the base And the attitude matrix of the base relative to the world coordinate system According to the rigid body rotation principle, the sensor installation inclination matrix can be calculated by the following formula:

[0060]

[0061] The original measurement value of the three-dimensional force sensor is s F i =[ s x i s y i s z i ] T , after the inclination compensation, the sensor measurement value in the base coordinate system is:

[0062]

[0063] Step 2: The posture of the end coordinate system in the base coordinate system is The coordinates of the ball joint in the tool coordinate system are: t r i =[ t xi t y i t z i ] T According to the translation theorem of force, the resultant force and moment of the end load in the base coordinate system are:

[0064]

[0065] Define the base inclination angle by first rotating the base around the Y axis of the world coordinate system by an angle θ, then rotating the base around the X axis of the world coordinate system by an angle Then the transformation matrix of the base coordinates relative to the world coordinate system is:

[0066]

[0067] When the attitude adjustment mechanism is in a stationary state, the relationship between gravity and the sensor measurement value is as follows:

[0068]

[0069] in, w G=[00-mg] T represents gravity, m is the load mass, g represents the acceleration of gravity, and the magnitude of gravity is The base installation inclination angle can be calculated by the following formula:

[0070]

[0071] After the base installation inclination compensation is performed, the gravity under the base coordinate system is:

[0072]

[0073] Step 3: Let the attitude adjustment mechanism run in free space according to the excitation trajectory. At this time, the external torque on the end load is zero and the acceleration is Angular velocity t ω, angular acceleration Center of mass coordinates r m , the resultant torque measured by the force sensor in the tool coordinate system can be expressed as:

[0074] t M=[LN][x] (40)

[0075] in:

[0076]

[0077] x=[r x r y r z I xx I yy Izz I xy I xz I yz ] T (43)

[0078] Formula (9) can be simplified to i =b i x, continuous data collection can be obtained:

[0079] A=Bx (44)

[0080] A=[a1 T a2 T …a n T ] T (45)

[0081] B=[b1 T b2 T …b n T ] T (46)

[0082] Kinetic parameter identification based on the least squares principle:

[0083] x=(B T B) -1 B T A (47)

[0084] Step 4: During the terminal motion, the external forces and torques are t f and t τ e , establish the dynamic model of the end load in the base coordinate system to compensate for the non-contact force and realize the load force perception:

[0085]

[0086] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

[0087] The present invention does not involve parts that are the same as the prior art or can be implemented by using the prior art.

Claims

1. A method for accurately sensing load force based on a parallel robot joint force sensor, characterized in that: The following steps are involved: S1: Use a laser tracker to measure the reflective target balls installed on the base and the socket respectively, fit and calculate the posture of the socket coordinate system relative to the base coordinate system, and complete the calibration of the force sensor installation inclination angle; S2: Construct a six-dimensional force solution model based on joint force sensors, and self-calibrate the installation inclination angle of the attitude adjustment mechanism base through the force measurement data of the attitude adjustment mechanism in static state; S3: Make the posture adjustment mechanism run according to the excitation trajectory, record the load motion data and the force value measured by the three-dimensional force sensor, and identify the load dynamic parameters online; S4: Compensate for the non-contact force caused by the load based on the dynamic model to achieve accurate force sensing of the end load.

2. The method according to claim 1, characterized in that: The coordinate system constructed in the step S1 of force sensor installation inclination calibration and the step S2 of base installation inclination calibration includes: a world coordinate system w -X w Y w Z w , its Z-axis direction is parallel to and opposite to gravity, and the coordinate system of the attitude adjustment mechanism base is O b -X b Y b Z b , the measurement coordinate system O of each joint force sensor si -X si Y si Z si , where i = {1, 2, 3}, the end load coordinate system O t -X t Y t Z t .

3. The method according to claim 1, characterized in that: The specific steps of the force sensor installation inclination calibration in step S1 are as follows: S1.1: Install a reflective target ball on the base of the attitude adjustment mechanism and use a laser tracker to measure. The coordinates of the reflective target ball in the base coordinate system are: b P i =[ b x i b y i b z i ], where i = {1, 2, 3, 4}, and the coordinates measured by the laser tracker are l P i =[ l x i l y i l z i ]; S1.2: Install a reflective target ball in the ball socket connected to the force sensor and use a laser tracker to measure. The coordinates of the reflective target ball in the force sensor measurement coordinate system are: l S i =[ l x i l y i b z i ], where i = {1, 2, 3}, and the coordinates measured by the laser tracker are l P i =[ l x i l y i l z i ]; S1.3: Use SVD decomposition to calculate the attitude matrix of the force sensor relative to the base And the attitude matrix of the base relative to the world coordinate system According to the rigid body rotation principle, the sensor installation inclination matrix is ​​calculated by the following formula: S1.4: The original measurement value of the three-dimensional force sensor is s F i =[ s x i s y i s z i ] T , after the inclination compensation, the sensor measurement value in the base coordinate system is:

4. The method according to claim 1, characterized in that: The step S2 of self-calibrating the installation inclination angle of the attitude adjustment mechanism base comprises the following steps: S2.1: The posture of the terminal coordinate system in the base coordinate system is The coordinates of the ball joint in the tool coordinate system are: t r i =[ t x i t y i t z i ] T According to the translation theorem of force, the resultant force and moment of the end load in the base coordinate system are: S2.2: Define the base inclination angle. First rotate the base around the Y axis of the world coordinate system by an angle θ, then rotate the base around the X axis of the world coordinate system by an angle Then the transformation matrix of the base coordinates relative to the world coordinate system is: S2.3: When the attitude adjustment mechanism is in a stationary state, the relationship between gravity and the sensor measurement value is as follows: in, w G=[0 0 -mg] T represents gravity, m is the load mass, g represents the acceleration of gravity, and the magnitude of gravity is The base installation inclination angle can be calculated by the following formula: S2.4: After the base installation inclination compensation, the gravity in the base coordinate system is:

5. The method according to claim 1, characterized in that: The step S3 of load dynamics parameter identification includes the following steps: S3.1: Let the attitude adjustment mechanism run in free space according to the excitation trajectory. At this time, the external torque on the end load is zero and the acceleration is Angular velocity t ω, angular acceleration Center of mass coordinates r m , the resultant torque measured by the force sensor in the tool coordinate system can be expressed as: t M=[L N][x] (8) in: x=[r x r y r z I xx I yy I zz I xy I xz I yz ] T (11) S3.2: Equation (9) can be simplified to i =b i x, continuous data collection can be obtained: A=Bx (12) <h2 style=";text-align:left;direction:ltr">A=[a1<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> a2<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> ...a<h2 style=";text-align:left;direction:ltr"> n <h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> ]<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> (13) B=[b1 T b2 T ...b n T ] T (14) Kinetic parameter identification based on the least squares principle: x=(B T B) -1 B T A (15)。 6. The method according to claim 1, characterized in that: The method for realizing force sensing by compensating non-contact force in step S4 is as follows: During the terminal motion, the external force and torque are t f and t τ e , establish the dynamic model of the end load in the base coordinate system to compensate for the non-contact force and realize the load force perception:

Images