A robot pose measurement device and method based on a six-wire encoder

Abstract

The present invention discloses a robot pose measurement device and method based on six wire-pulling sensors. The device consists of a disc base, a disc base bracket, an upper disc seat, a wire-pulling encoder, an encoder bracket, a pulley bracket, a fixed pulley, a movable pulley, a movable pulley bracket, a pedestal bearing, a magnetic angle encoder, and a universal joint. The wire heads of the six wire-pulling encoders are connected to the six universal joints of the upper disc seat. When the spatial pose of the upper disc seat changes, the wire length, the deflection angle of the movable pulley bracket, and the wrap angle will all change. From this, the spatial positions of six points can be calculated, and then the pose of the upper disc seat can be solved based on the positions of the six points in the world coordinate system and the target coordinate system. The present invention can achieve rapid measurement of the pose of the end of an industrial robot, and at the same time, the device has the advantages of low price and simple installation.

Description

Technical Field

[0001] The invention belongs to the technical field of robot pose measurement, and particularly relates to a robot pose measurement device and method based on a six-wire encoder. Background Art

[0002] Since the birth of industrial robots, continuous breakthroughs and innovations have been made. At present, industrial robots have been widely used in fields such as aviation, aerospace, automotive manufacturing, and service industries due to their advantages of strong reliability, high repeat positioning accuracy, good programmability, and high efficiency. With the advent of the industrial 4.0 era, the application of industrial robots is more extensive, and higher requirements are put forward for the performance and accuracy of industrial robots. The accuracy indicators of industrial robots are mainly divided into two aspects: absolute positioning accuracy and repeat positioning accuracy. Among them, the absolute positioning accuracy of the robot is the core of offline programming technology. With the progress of science and technology and manufacturing processes, the repeat positioning accuracy of current robots has reached a relatively high level.

[0003] At present, whether it is large factories or small and medium-sized enterprises, robot equipment has been gradually introduced in recent years, and the application of robots in various industries has been relatively common. As the working time of industrial robots becomes longer, the repeat positioning accuracy and absolute positioning accuracy of the robots will both show varying degrees of performance degradation. Currently, the way to improve the accuracy performance of industrial robots is through calibration technology. The calibration technology of industrial robots is divided into four basic steps: modeling, measurement, identification, and compensation. Among them, the measurement step is to accurately detect the end position and attitude data of the robot through external measurement equipment, which is the key to the calibration effect. Currently, the measurement of the robot end pose is mainly achieved through high-precision equipment such as stereo vision devices, laser trackers, and laser interferometers. Such equipment usually costs hundreds of thousands or even millions. Therefore, there is an urgent need to propose a pose measurement device and method with low cost and high detection accuracy to meet the current requirements of robot pose measurement. Summary of the Invention

[0004] In view of the deficiencies in the prior art, the invention provides a robot pose measurement device and method based on a six-wire encoder.

[0005] To achieve the above object, the invention adopts the following technical solutions:

[0006] A robot pose measurement device based on a six-wire encoder, characterized by comprising: a disc base, a wire encoder, a fixed pulley, a movable pulley, a pulley bracket, a pedestal bearing, a universal joint, and a disc upper seat;

[0007] Six wire rope encoders are evenly distributed along the circumference on the disc base, and the extension lines of the wire ropes of each wire rope encoder intersect at the midpoint of the disc base; a corresponding pulley bracket is installed on one side of each wire rope encoder. One end of the pulley bracket is equipped with a fixed pulley, the bottom of the fixed pulley is horizontally tangent to the wire rope pulled out by the wire rope encoder, the other end of the pulley bracket is installed with a pedestal bearing, the movable pulley is installed on the pedestal bearing, and the rotation axis of the pedestal bearing, the lower tangent line of the movable pulley and the upper tangent line of the fixed pulley always remain on the same straight line. The wire rope of the wire rope encoder passes through the fixed pulley and the movable pulley in sequence and is pulled out; six universal joints are evenly installed at the bottom of the upper disc seat, and the universal joints correspond to the wire rope encoders one by one. The wire rope head of the wire rope encoder is connected to the corresponding universal joint.

[0008] To optimize the above technical solution, the specific measures taken also include:

[0009] Further, the wire rope encoder is fixed on the disc base through an encoder bracket. The encoder bracket is an L-shaped structure, and threaded holes are provided at the bottom and side of the encoder bracket for connecting and fixing the disc base and the wire rope encoder respectively.

[0010] Further, the movable pulley is fixed on the pedestal bearing through a movable pulley bracket, and a magnetic angle encoder is also installed on the pulley bracket to measure the rotation angle of the movable pulley bracket.

[0011] At the same time, the present invention also proposes a robot pose measurement method based on six wire rope encoders. Using the above-mentioned robot pose measurement device, it is characterized in that it includes the following steps:

[0012] Step 1: Fix the upper disc seat to the end of the measured robot.

[0013] Step 2: When the measured robot moves, the movable pulley rotates around the rotation axis, introducing the wire rope envelope angle, the rotation angle of the movable pulley and the wire rope length, and solving the spatial position information of the measured point.

[0014] Further, in Step 1, point O is defined as the wire rope outlet point of the wire rope encoder, point A is the lower tangent point of the wire rope and the fixed pulley, point B is the upper tangent point of the wire rope and the fixed pulley, point C is the lower tangent point of the wire rope and the movable pulley, and point D is the upper tangent point of the wire rope and the movable pulley. Among them, point O and point A are horizontally collinear, point B and point C are horizontally collinear, point B and point C are on the rotation axis of the movable pulley, and the position of the lower tangent point of the movable pulley remains unchanged.

[0015] Further, Step 2 specifically includes the following steps:

[0016] Step 1: Establish a measurement model of a single wire rope encoder and establish a spatial coordinate system in the calibration system.

[0017] Step 2: Introduce the wire-pulling envelope angle, the rotating angle of the movable pulley, and the wire-pulling length, and calculate the position of the center point of the universal joint on the upper seat of the disc in the coordinate system corresponding to its wire-pulling encoder;

[0018] Step 3: According to the coordinates of the center point of the universal joint on the upper seat of the disc corresponding to each wire-pulling encoder obtained in Step 2, and combined with the intelligent evolutionary algorithm, solve the pose of the central coordinate system of the upper seat of the disc in the central coordinate system of the disc base.

[0019] Further, the specific content of Step 1 is as follows:

[0020] In the movable pulley, establish an OXYZ space rectangular coordinate system at point C. At the initial position, establish O C X C Y C Z C coordinate system T C , with the wire-pulling CB as the X C axis, the direction perpendicular to the disc base upward as the Z C axis, and the Y C direction obtained by the right-hand screw rule; at the initial position, establish O D X D Y D Z D coordinate system T D , with the direction of the wire-pulling DP as the X D direction, the direction perpendicular to the DP direction upward as the Z D direction, and the Y D direction obtained by the right-hand screw rule.

[0021] Further, the specific content of Step 2 is as follows:

[0022] Use the homogeneous transformation equation to calculate the coordinates of the center point P of the universal joint on the upper seat of the disc in the coordinate system T C . Introduce the wire-pulling envelope angle α and the rotating angle β of the movable pulley. The length of the wire-pulling from the wire outlet of the wire-pulling encoder to the lower tangent point C of the movable pulley is denoted as L0, the radius of the movable pulley is denoted as r, and the length from point D to point P is |DP|. Calculate the wire-pulling length L according to the following formula (1.1):

[0023] L = r·α + |DP| (1.1)

[0024] First, rotate the coordinate system T C around the X C axis by β angles to the position where the Y C axis is perpendicular to the plane where the movable pulley is located, then move along the X C axis direction by d1 distance, then move along the rotated Z C axis direction by d2 distance, and rotate around the moved Y C axis by γ angles to obtain the coordinate system T D, finally move along the positive X D direction by a distance of |DP| to obtain the center point P of the universal joint on the upper disc P X P Y P Z P coordinate system T P ;

[0025] Represent T using a homogeneous coordinate matrix C to T P The transformation relationship between them is:

[0026]

[0027] In Equation (1.2):

[0028]

[0029] Substitute Equation (1.3) into Equation (1.2) to get:

[0030]

[0031] Determine the transformation relationship between the coordinate system T of the center point P of the universal joint on the upper disc P and the coordinate system T C between them

[0032]

[0033] In the formula, represents the rotation matrix of the center point P of the universal joint on the upper disc in the coordinate system T C below, represents the position of the center point P of the universal joint on the upper disc in the coordinate system T C below;

[0034] Therefore, the coordinates P of the center point P of the universal joint on the upper disc in T C are expressed as: C

[0035]

[0036] Equation (1.6) is the position solution formula of the center point P of the universal joint on the upper disc in the coordinate system corresponding to its wire rope encoder.

[0037] Furthermore, the specific steps of step 3 are as follows:

[0038] Define the central coordinate system of the disc base as T1 and the central coordinate system of the upper disc as T2, where the O of each wire rope encoder Ci X Ci Y Ci Z Ci ​The transformation matrix between the coordinate system and the central coordinate system T1 of the disc base is H 1i , and the center point P of the universal joint corresponding to each wire rope encoder i The transformation matrix of the point in the central coordinate system T2 of the upper disc seat is H 2i ;

[0039] According to Step 2, the center point P of the universal joint corresponding to each wire rope encoder is obtained i :

[0040]

[0041] In the formula, α i represents the wire rope envelope angle of each wire rope encoder;

[0042] Convert the coordinates of the above universal joint center point P i to the position in the T1 coordinate system:

[0043]

[0044] Define the pose of the central coordinate system T2 of the upper disc seat in the central coordinate system T1 of the disc base as (x, y, z, μ, ν), and the corresponding attitude matrix is as follows:

[0045]

[0046] In the formula, R z (υ), R y (μ), represent rotations of ν, μ, respectively, about the Z, Y, X axes of the coordinate system T1, angle; c represents cos, s represents sin, μ, ν, represent the rotation angles about the Y, Z, X axes respectively;

[0047] The pose matrix of the central coordinate system T2 of the upper disc seat in the central coordinate system T1 of the disc base is as follows:

[0048]

[0049] In the formula, p = (x, y, z);

[0050] The coordinates of the universal joint center point P i P″ i are expressed according to T2 and H 2i , P′ i and P″ i are both expressed in the central coordinate system T1 of the disc base through the wire rope encoder measurement model and the end pose calculation, where P′ i and P″i respectively include six unknown parameters (α1, α2, α3, α4, α5, α6) and a total of 12 unknown parameters; solve the above 12 unknown parameters through an intelligent evolutionary algorithm, and the finally solved pose is

[0051] Furthermore, the intelligent evolutionary algorithm is a genetic algorithm or a particle swarm algorithm.

[0052] The beneficial effects of the present invention are as follows: the present invention can achieve rapid measurement of the pose of the end of an industrial robot, and at the same time, the device has the advantages of low price and simple installation. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 is a top view of the robot pose measurement device of the six-wire encoder of the present invention.

[0054] Figure 2 is a front view of the robot pose measurement device of the six-wire encoder of the present invention.

[0055] Figure 3 is an installation schematic diagram of the pulley bracket, wire encoder, encoder bracket and disc base of the present invention.

[0056] Figure 4 is an installation schematic diagram of the base bracket and the disc base of the present invention.

[0057] Figure 5 is the installation relationship between the universal joint and the upper disc seat of the present invention.

[0058] Figure 6 is an installation schematic diagram of the pulley bracket assembly of the present invention.

[0059] Figure 7 is an installation schematic diagram of the movable pulley bracket and the pillow block bearing of the present invention.

[0060] Figure 8 is an installation schematic diagram of the movable pulley bracket and the magnetic encoder magnet of the present invention.

[0061] Figure 9 is a schematic diagram of the disc base of the present invention.

[0062] Figure 10 is a schematic diagram of the upper disc seat of the present invention.

[0063] Figure 11 is a schematic diagram of the pulley bracket of the present invention.

[0064] Figure 12 is a schematic diagram of the movable pulley bracket of the present invention.

[0065] Figure 13It is a schematic diagram of the encoder bracket of the present invention.

[0066] Figure 14 It is a schematic diagram of the pulling wire path of the single-pulling wire encoder of the present invention.

[0067] Figure 15 It is a schematic diagram of the coordinate system of the single-pulling wire encoder of the present invention.

[0068] Figure 16 It is a schematic diagram of the relationship between the coordinate system of the single-pulling wire encoder and the pose of the robot. Detailed implementation manners

[0069] Now, the present invention will be further described in detail with reference to the accompanying drawings.

[0070] See Figures 1 to 4 As shown, the present invention is a robot pose measurement device for a six-pulling wire encoder, which is applied to measure the end pose of an industrial robot. The main parts include a disc base 1, six pulling wire encoders, encoder brackets, fixed pulleys, movable pulleys, pulley brackets, movable pulley brackets, pillow block bearings, universal joints, and an upper disc 2. The disc base 1 mainly includes pulling wire encoders 113, 114, 115, 116, 117, 118, which are evenly distributed above the disc base 1 to ensure that the extension lines of the pulling wires intersect at the midpoint of the disc base 1. Encoder brackets 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112 are installed behind and to the right of the pulling wire encoders to fix the pulling wire encoders on the disc base 1; the encoder brackets are L-shaped structures with two threaded holes at the lower and side respectively, which are used to connect and fix the disc base 1 and the pulling wire encoders respectively, and the size of the threaded holes is designed according to the size of the screw holes of the pulling wire encoders themselves. Pulley bracket assemblies 119, 120, 121, 122, 123, 124 are installed directly in front of the pulling wire encoders. The bottom of the side of the pulley bracket assemblies close to the pulling wire encoders needs to be cut off to leave space for the pulling wires of the pulling wire encoders to be pulled out smoothly. Disc base brackets 125, 126, 127, 128, 129, 130 are evenly installed below the disc base 1.

[0071] See Figure 5 As shown, the upper disc 2 mainly includes universal joints 201, 202, 203, 204, 205, 206, which are evenly distributed below the upper disc 2 and are fixed to the pulling wire heads of the pulling wire encoders one by one with bolts. A hole is drilled at the middle position of the upper disc 2 according to the position of the threaded hole at the end of the robot, and then it is installed at the end of the robot with bolts.

[0072] See Figure 6As shown, the pulley support assembly 119 mainly includes a pulley support 11901, a fixed pulley 11902, a movable pulley 11903, a movable pulley support 11904, a seat bearing 11905, and a magnetic angle encoder 11906. The center lines of the fixed pulley 11902, the movable pulley 11903 and the seat bearing 11905 should be on the same line, so that when the spatial position of the measured object changes, the pull line always rotates around the rotation axis to ensure the measurement accuracy. At the same time, the upper tangent of the fixed pulley 11902, the lower tangent of the movable pulley 11903 and the center line of the seat bearing should coincide, and the lower tangent of the fixed pulley 11902 should coincide with the horizontal pull line of the pull line encoder 113, so that the pull line can be pulled out horizontally, avoiding unnecessary measurement errors and further ensuring the measurement accuracy. See Figure 7 and Figure 8 As shown, the movable pulley bracket 11904 and the seat bearing 11905 are fixed by the fastening screws of the seat bearing, and the rear of the movable pulley bracket 11904 is magnetically attracted to the magnetic block 11907 of the magnetic angle encoder. The magnetic angle encoder 11906 is used to measure the rotation angle β of the movable pulley bracket.

[0073] Figures 9 to 13 They are schematic diagrams of the disc base, the disc upper seat, the pulley bracket, the movable pulley bracket and the encoder bracket of the present invention respectively.

[0074] See also Figures 14 to 16 ,The measurement method of the end position of the robot to be measured is divided into the following steps:

[0075] Step 1: Fix the upper seat of the disc to the end of the robot under test, define point O as the outlet point of the cable encoder, point A as the lower tangent point between the cable and the fixed pulley, point B as the upper tangent point between the cable and the fixed pulley, point C as the lower tangent point between the cable and the movable pulley, and point D as the upper tangent point between the cable and the movable pulley. Point O and point A are horizontally collinear to reduce friction with the cable opening and extend the service life of the device. Point B and point C are horizontally collinear. Point B and point C are on the rotating axis of the movable pulley. Due to the existence of the rotating bearing, when the spatial position of the object under test changes, the movable pulley bracket will rotate along the rotating axis to ensure that the lower tangent point of the movable pulley remains unchanged.

[0076] Step 2: When the robot under test moves, the movable pulley rotates around the rotation axis, and the envelope angle and the length of the pull line on the movable pulley will change. The spatial position information of the measured point can be solved by introducing the three parameters of envelope angle, rotation angle, and pull line length. The specific steps are as follows:

[0077] Step 1: Establish the measurement model of the single-wire encoder. To express the three-dimensional coordinates of the center point P of the universal joint on the upper disc of the disc, it is first necessary to establish a spatial coordinate system in the calibration system. The positions of the lower tangent point A and the upper tangent point B remain unchanged during the measurement process. In the movable pulley, the position of the lower tangent point C remains unchanged. Therefore, a spatial rectangular coordinate system OXYZ is established at point C. At the initial position, establish O C X C Y C Z C coordinate system, with the wire CB as the X C axis (pointing from point C to point B), the vertically upward direction as the ZC axis. According to the right-hand screw rule, the Y C direction is perpendicular to the paper and outward. Point D is the upper tangent point of the movable pulley, and its position changes with the position of the center point P of the universal joint on the upper disc of the disc. At the initial position, establish O D X D Y D Z D coordinate system at point D, with the direction of the wire DP as the X D direction (pointing from point D to point P), the direction perpendicular to DP and upward as the Z D direction. According to the right-hand screw rule, the Y D direction.

[0078] Step 2: Calculate the coordinates of the center point P of the universal joint on the upper disc of the disc in the coordinate system O C X C Y C Z C using the homogeneous transformation equation. Introduce the wire envelope angle α and the movable pulley rotation angle β. The length of the wire from the encoder outlet to the lower tangent point C of the movable pulley remains unchanged, denoted as L0. The radius of the movable pulley is denoted as r. The length L measured by the encoder mainly includes the following aspects: the envelope arc length, the length |DP| from point D to point P. According to Equation (1.1), the wire length L can be accurately calculated.

[0079] L = r·α + |DP| (1.1)

[0080] During the calibration process, the movable pulley rotates along the rotation axis, and a movable pulley rotation angle β also needs to be introduced. The lower tangent point C, the upper tangent point D, and the center point P of the universal joint on the upper disc of the disc are always in the same plane. First, rotate the coordinate system O C X C Y C Z C (coordinate system T C ) by β angle around the X C axis to the position where the Y C axis is perpendicular to the plane where the movable pulley is located, then move a distance d1 along the X C axis direction, and then move along the rotated Z CMove a distance d2 in the axial direction, and then rotate by an angle γ around the moved Y C axis to obtain the coordinate system O D X D Y D Z D axis, and finally move a distance |DP| in the positive direction of the X D axis to obtain the coordinate system O P X P Y P Z P (coordinate system TP).

[0081] Use the homogeneous coordinate matrix to represent the transformation relationship between T C to T P :

[0082]

[0083] The parameters in Equation (1.2) can be expressed as:

[0084]

[0085] Substitute Equation (1.3) into (1.2) to obtain:

[0086]

[0087] From Equation (1.4), the transformation relationship between the center point P of the universal joint on the upper disc and the coordinate system T P and the coordinate system T C can be determined

[0088] In the formula, represents the rotation matrix of the center point P of the universal joint on the upper disc in the coordinate system T C , and is the position.

[0089] Therefore, the coordinates of the center point P of the universal joint on the upper disc in T C can be expressed as:

[0090]

[0091] Equation (1.6) is the position solution formula of the center point P of the universal joint on the upper disc in the coordinate system corresponding to its wire encoder. Only the envelope angle α is the unknown parameter, while the rotation angle β of the movable pulley and the wire length L can both be measured.

[0092] Step 3: According to the configuration of the system, there are a total of six wire encoders. Define the central coordinate system of the disc base as T1 and the central coordinate system of the upper disc as T2 for the entire measurement system. Among them, the O of each wire encoderCi X Ci Y Ci Z Ci The transformation matrix between the coordinate system and the central coordinate system T1 of the disc base is H 1i , and the center point P of the universal joint on the upper disc corresponding to each wire-pulling encoder i The transformation matrix of the point in the central coordinate system T2 of the upper disc is H 2i . The pose of the T2 coordinate system in the T1 coordinate system is the quantity to be solved. According to Step 2, the center point P of the universal joint on the upper disc corresponding to each wire-pulling encoder i , and its corresponding coordinates can be calculated by Equation (1.6), as shown below:

[0093]

[0094] Only α i parameter is the unknown. Convert the coordinates of the above universal joint center point P i to the position in the T1 coordinate system:

[0095]

[0096] Define the pose of the central coordinate system T2 of the upper disc in the central coordinate system T1 of the disc base as (x, y, z, μ, ν), and the corresponding attitude matrix is as follows:

[0097]

[0098] In the formula, R z (υ), R y (μ), represent rotations of ν, μ, around the Z, Y, X axes of the coordinate system T1 respectively, angle; c represents cos, s represents sin, μ, v, represent the rotation angles around the Y, Z, X axes respectively.

[0099] And the pose matrix of the central coordinate system T2 of the upper disc in the central coordinate system T1 of the disc base is as follows:

[0100]

[0101] In the formula, p = (x, y, z).

[0102] Therefore, the coordinates of the universal joint center point P i can be expressed according to T2 and H 2i , and are expressed as P″ i , P′ i and P″ iThey are calculated through the measurement model of the wire encoder and calculated based on the end pose respectively, both expressed in the central coordinate system T1 of the disc base, where P′ i and P″ i respectively contain 6 unknowns (α1, α2, α3, α4, α5, α6) and a total of 12 unknown parameters. For P′ i and P″ i where i = 6, a total of 18 equations can be established. However, due to the existence of non-linear functions such as trigonometric functions in the coordinates, algorithms such as the least squares method cannot be directly used for solution. The 12 unknown parameters can be solved through intelligent evolutionary algorithms, such as genetic algorithms, particle swarm algorithms, etc. The finally solved pose is

[0103] The above is only the preferred implementation mode of the present invention. The protection scope of the present invention is not limited to the above embodiments. All technical solutions falling within the idea of the present invention belong to the protection scope of the present invention. It should be pointed out that for those of ordinary skill in the art, several improvements and refinements made without departing from the principle of the present invention should be regarded as within the protection scope of the present invention.

Claims

1. A robot pose measurement method based on a six-wire encoder, characterized in that, The adopted robot pose measurement device includes: a disc base, a wire-drawing encoder, a fixed pulley, a movable pulley, a pulley bracket, a pedestal bearing, a universal joint and an upper disc; six wire-drawing encoders are evenly distributed on the disc base in the circumferential direction, and the extension lines of the wires of each wire-drawing encoder intersect at the midpoint of the disc base; a corresponding pulley bracket is installed on one side of each wire-drawing encoder. One end of the pulley bracket is installed with a fixed pulley, the bottom of the fixed pulley is horizontally tangent to the wire drawn out by the wire-drawing encoder, the other end of the pulley bracket is installed with a pedestal bearing, the movable pulley is installed on the pedestal bearing, and the rotation axis of the pedestal bearing, the lower tangent line of the movable pulley and the upper tangent line of the fixed pulley always remain on the same straight line, and the wire of the wire-drawing encoder passes through the fixed pulley and the movable pulley in sequence and is drawn out; six universal joints are evenly installed at the bottom of the upper disc, the universal joints correspond to the wire-drawing encoders one by one, and the wire head of the wire-drawing encoder is connected to the corresponding universal joint; The robot pose measurement method includes the following steps: Step 1: Fix the upper disc to the end of the measured robot; define point O as the wire outlet point of the wire-drawing encoder, point A as the lower tangent point of the wire and the fixed pulley, point B as the upper tangent point of the wire and the fixed pulley, point C as the lower tangent point of the wire and the movable pulley, and point D as the upper tangent point of the wire and the movable pulley. Among them, point O and point A are horizontally collinear, point B and point C are horizontally collinear, point B and point C are on the rotation axis of the movable pulley, and the position of the lower tangent point of the movable pulley remains unchanged; Step 2: When the measured robot moves, the movable pulley rotates around the rotation axis, introducing the wire envelope angle, the movable pulley rotation angle and the wire length, and solving the spatial position information of the measured point; Step 2 specifically includes the following steps: Step 1: Establish the measurement model of the single-wire encoder and establish a spatial coordinate system in the calibration system; in the movable pulley, establish a right-handed rectangular coordinate system OXYZ at point C. At the initial position, establish O C X C Y C Z C coordinate system T C , with the wire CB as the X C axis, the direction vertically upward from the disc base as the Z C axis, and the Y C direction obtained by the right-hand screw rule; at the initial position, establish O D X D Y D Z D coordinate system T D , with the direction of the wire DP as the X D direction, the direction vertically upward from the DP direction as the Z D direction, and the Y D direction obtained by the right-hand screw rule; Step 2: Introduce the wire-pulling envelope angle α, the rotating angle β of the movable pulley, and the wire-pulling length L, and calculate the coordinates P of the center point P of the universal joint on the upper seat of the disc at T C under C Expressed as: In the formula, the radius of the movable pulley is denoted as r; Step 3: According to the coordinates of the center point of the universal joint of the upper disc corresponding to each wire-drawing encoder obtained in Step 2, combined with the intelligent evolutionary algorithm, solve the pose of the center coordinate system of the upper disc in the center coordinate system of the disc base; The specific steps of Step 3 are as follows: Define the central coordinate system of the disk base as T1 and the central coordinate system of the upper disk as T2. Among them, the O of each wire rope encoder Ci X Ci Y Ci Z Ci The transformation matrix between the coordinate system and the central coordinate system T1 of the disk base is H 1i , and the transformation matrix of the center point P of the cardan joint corresponding to each wire rope encoder i point in the central coordinate system T2 of the upper disk is H 2i ; Obtain the center point P of the universal joint on the upper disc corresponding to each wire drawing encoder according to Step 2 i : where α i represents the wire-drawing envelope angle of each wire-drawing encoder; Convert the coordinates of the center point P of the universal joint above i to the position in the T1 coordinate system: Define the pose representation of the central coordinate system T2 of the upper seat of the disc under the central coordinate system T1 of the disc base as The corresponding attitude matrix is shown as follows: wherein, R z (υ), R y (μ), represent rotations about the Z, Y, and X axes of the coordinate system T1 by the angles; c represents cos, s represents sin, representing the rotation angles about the respective Y, Z, and X axes; The pose matrix of the center coordinate system T2 of the upper disc in the center coordinate system T1 of the disc base is shown as follows: In the formula, p = (x, y, z); The center point P of the universal joint i coordinates P i "According to T2 and H 2i expression, P i ' and P i " are respectively obtained through the wire encoder measurement model and the end - pose - based calculation, both expressed in the central coordinate system T1 of the disc base, where P i ' and P i " respectively contain 6 unknown parameters (α1, α2, α3, α4, α5, α6) and a total of 12 unknown parameters; solve the above 12 unknown parameters through the intelligent evolutionary algorithm, and the finally solved pose is 2. The robot pose measurement method based on a six-wire encoder according to claim 1, wherein: The intelligent evolutionary algorithm is a genetic algorithm or a particle swarm algorithm.

3. A robot pose measurement method based on a six-wire encoder according to claim 1, characterized in that: The wire-drawing encoder is fixed on the disc base through an encoder bracket. The encoder bracket is an L-shaped structure, and threaded holes are provided at the bottom and side of the encoder bracket respectively for connecting and fixing the disc base and the wire-drawing encoder.

4. The method for measuring the pose of a robot based on a six-wire encoder according to claim 1, characterized in that: The movable pulley is fixed on the pedestal bearing through a movable pulley bracket, and a magnetic angle encoder is also installed on the pulley bracket to measure the rotation angle of the movable pulley bracket.

Images