A Sensing Method for Quaternion Joints and Equivalent Rotation Angles of the Wrist of a Cable-Driven Robotic Arm

Abstract

This invention relates to a method for sensing the quaternion joints of the wrist of a cable-driven robotic arm and their equivalent rotation angles. The method includes: a base, in which a first joint encoder and a second joint encoder are installed; a first branch, which is a four-bar linkage; a second branch, which is also a four-bar linkage; a third branch, which is also a four-bar linkage; and a wrist seat, where the second ends of the first branch, the second branch, and the third branch are all connected to a second plane of the wrist seat. This method ensures that the cable-driven agile spatial robotic arm can obtain feedback on the wrist joint position during motion planning and control, and that this feedback corresponds to the joint angle motion planning control parameters, thus creating the prerequisite for achieving closed-loop joint control of the robotic arm's wrist joint.

Description

Technical Field

[0001] This invention belongs to the field of rope-driven agile spatial robotic arms, and specifically relates to a method for sensing the quaternion joints of the wrist of a rope-driven robotic arm and its equivalent rotation angle. Background Technology

[0002] Traditional rigid robotic arms have a simple structure, with motors, reducers, and other structures placed at the joints to achieve joint movement. The weight of the robotic arm itself is evenly distributed on the arm. In contrast, rope-driven agile robotic arms do not have motors at the joints. Instead, the motors, reducers, and other structures are placed at the base of the robotic arm via rope transmission. This greatly reduces the mass and moment of inertia of the end effector, allowing the robotic arm to achieve greater speed and acceleration performance.

[0003] Because the structure of rope-driven robotic arms differs from that of traditional rigid arms, most rope-driven robotic arms do not have rotary joints built into the drive components. In particular, the quaternion wrist joint structure and the three-link rope-driven structure realize the pitch and yaw degrees of freedom of the wrist. However, there is no physical axis of rotation corresponding to its joint degrees of freedom. Therefore, the equivalent joint angle cannot be measured directly by installing a joint encoder at the joint as in traditional robotic arms. Summary of the Invention

[0004] This invention provides a method and apparatus for sensing the equivalent rotation angle of the quaternion joint of the wrist of a rope-driven robotic arm, aiming to at least solve one of the technical problems existing in the prior art.

[0005] The technical solution of this invention relates to a method for sensing the quaternion joint of the wrist of a cable-driven robotic arm and its equivalent rotation angle, the device comprising:

[0006] A base, in which a first joint encoder and a second joint encoder are installed;

[0007] The first branch chain is a four-bar linkage chain, and the first end of the first branch chain passes through the base and is connected to the first joint encoder.

[0008] The second branch is a four-bar linkage, and the first end of the second branch passes through the base and is connected to the second joint encoder.

[0009] The third branch is a four-bar linkage, and the first end of the third branch is fixed on the first plane of the base;

[0010] The first branch, the second branch, and the third branch are cyclically symmetrically distributed;

[0011] The wrist base has its second end, the second end of the first branch, and the second end of the third branch all connected to the second plane of the wrist base.

[0012] Specifically, the wrist joint mainly consists of a wrist base, a base, a first branch chain, a second branch chain, a third branch chain, a rope pulley, and an encoder. The wrist base and the base are supported and connected by three branches, which serve as a linkage structure to ensure that the upper and lower planes of the wrist joint maintain a ball-and-socket engagement motion. The encoder is installed on the base, and through a four-bar linkage structure at the connection point between the three branches and the first plane of the base, the motion of the bottom shaft of the branch chain is transmitted to the encoder shaft to achieve angle measurement.

[0013] Furthermore, the first branch includes a first branch first link, a first branch second link, and a first branch third link connected in sequence. The first end of the first branch first link is connected to the first joint encoder through a first branch seat disposed on the first plane of the base, and the second end of the first branch third link is connected to the wrist seat through a fourth branch seat disposed at the bottom of the wrist seat.

[0014] The second branch includes a second branch first link, a second branch second link, and a second branch third link connected in sequence. The first end of the second branch first link is connected to the second joint encoder through a second branch seat disposed on the first plane of the base. The second end of the second branch third link is connected to the wrist seat through a fifth branch seat disposed at the bottom of the wrist seat.

[0015] The third branch includes a first link, a second link, and a third link connected in sequence. The first end of the first link is connected to the third branch seat via a third branch seat disposed on the first plane of the base, and the second end of the third link is connected to the wrist seat via a sixth branch seat disposed at the bottom of the wrist seat.

[0016] Furthermore, the first plane of the base is provided with four sides, and each side is provided with a first drive rope seat, a second drive rope seat, a third drive rope seat and a fourth drive rope seat for connecting the drive rope;

[0017] The second plane of the wrist seat has four sides, and each side is provided with a fifth drive rope seat, a sixth drive rope seat, a seventh drive rope seat and an eighth drive rope seat for connecting the drive rope.

[0018] The first drive rope seat and the fifth drive rope seat are correspondingly arranged, the second drive rope seat and the sixth drive rope seat are correspondingly arranged, the third drive rope seat and the seventh drive rope seat are correspondingly arranged, and the fourth drive rope seat and the eighth drive rope seat are correspondingly arranged.

[0019] Furthermore, the bottom of the base also includes a first guide seat, a second guide seat, a third guide seat, and a fourth guide seat, all of which are positioned opposite each other on the same side of the first drive rope seat, the second drive rope seat, the third drive rope seat, and the fourth drive rope seat.

[0020] Furthermore, it also includes a first drive rope, a second drive rope, a third drive rope, and a fourth drive rope, the first ends of which are all pulled out from the drive box;

[0021] The first drive rope passes sequentially through the first pulley of the first guide seat, the first pulley of the first drive rope seat, the first pulley of the fifth drive rope seat, the second pulley of the fifth drive rope seat, the second pulley of the first drive rope seat, and the second pulley of the first guide seat before returning to the drive box;

[0022] The second drive rope passes sequentially through the first pulley of the second guide seat, the first pulley of the second drive rope seat, the first pulley of the sixth drive rope seat, the second pulley of the sixth drive rope seat, the second pulley of the second drive rope seat, and the second pulley of the second guide seat before returning to the drive box;

[0023] The third drive rope passes sequentially through the first pulley of the third guide seat, the first pulley of the third drive rope seat, the first pulley behind the seventh drive rope seat, the second pulley behind the seventh drive rope seat, the second pulley of the third drive rope seat, and the second pulley of the third guide seat before returning to the drive box;

[0024] The fourth drive rope passes sequentially through the first pulley of the fourth guide seat, the first pulley of the fourth drive rope seat, the first pulley behind the eighth drive rope seat, the second pulley behind the eighth drive rope seat, the second pulley of the fourth drive rope seat, and the second pulley of the fourth guide seat before returning to the drive box.

[0025] Furthermore, the quaternion joints of the wrist of the cable-driven robotic arm are equivalent to the first rotary joint, the second rotary joint, the first pitch joint, and the second pitch joint.

[0026] The first rotary joint and the second rotary joint have the same rotation angle but opposite directions. The rotation angle of the first rotary joint and the second rotary joint is the azimuth angle.

[0027] The first pitch joint and the second pitch joint have the same rotation angle in both magnitude and direction, and the rotation angle of the first pitch joint and the second pitch joint is the tilt angle.

[0028] Furthermore, this invention also proposes a method for sensing the equivalent rotation angle of a quaternion joint in the wrist of a cable-driven robotic arm, applied to the quaternion joint of the wrist of a cable-driven robotic arm. The method includes the following steps:

[0029] S100. Construct the branch configuration equations of the first branch, the second branch, and the third branch to obtain the joint kinematic model;

[0030] S200. Determine the orientation angle and tilt angle of the quaternion joint of the robotic arm wrist by using the end position of the quaternion joint of the robotic arm wrist.

[0031] S300. Based on the joint kinematics model, calculate the positions of the second end of the first branch, the second end of the second branch, and the second end of the third branch;

[0032] S400: Calculates end position, azimuth, and tilt angle based on numerical fitting.

[0033] Furthermore, in step S100,

[0034] The branch configuration equation is:

[0035]

[0036] in, , and respectively Let be the homogeneous transformation matrix of the coordinate systems of the ends of the first, second, and third branches relative to the base coordinate system. , , These represent the circumferential distribution phases of the first branch, the second branch (300), and the third branch relative to the first plane, respectively. These are the angles between adjacent links of the first branch chain, namely the angle between the first branch chain seat and the first link of the first branch chain, the angle between the first link of the first branch chain and the second link of the first branch chain, the angle between the second link of the first branch chain and the third link of the first branch chain, and the angle between the third link of the first branch chain and the fourth branch chain seat. These are the angles between adjacent links of the second branch, namely the angle between the second branch seat and the first link of the second branch, the angle between the first link of the second branch and the second link of the second branch, the angle between the second link of the second branch and the third link of the second branch, and the angle between the third link of the second branch and the fifth branch seat. These are the angles between adjacent links of the third branch chain, namely the angle between the third branch chain seat and the first link of the third branch chain, the angle between the first link of the third branch chain and the second link of the third branch chain, the angle between the second link of the third branch chain and the third link of the third branch chain, and the angle between the third link of the third branch chain and the sixth branch chain seat.

[0037] Furthermore, in step S200, the orientation angle and tilt angle of the quaternion joint of the wrist of the cable-driven robotic arm are:

[0038]

[0039] Where φ is the orientation angle of the quaternion joint, θ is the tilt angle of the quaternion joint, xe is the x-component of the quaternion joint end position, ye is the y-component of the quaternion joint end position, and ze is the z-component of the quaternion joint end position.

[0040] Furthermore, in step S300,

[0041] The end position of the second end of the first branch is:

[0042]

[0043] Where pae is the end position of the second end of the first branch. The distance from the center of the plane to the connection point between the first and fourth branch chain seats and the upper and lower planes. The distance between the common perpendicular lines of the first branch chain seat pivot and the first link pivot of the first branch chain.

[0044] Compared with existing technologies, the present invention has the following characteristics:

[0045] This invention provides a method for sensing the equivalent rotation angle of the wrist quaternion joint in a rope-driven agile spatial manipulator. This ensures that the rope-driven agile spatial manipulator can obtain feedback on the wrist joint position during motion planning and control, and that this feedback corresponds to the joint angle motion planning control parameters, creating the prerequisite for achieving closed-loop joint control of the manipulator's wrist joint. The encoder is mounted on a branch shaft connected to the base, with the shaft position fixed relative to the base, facilitating rear-mounting and significantly reducing the wrist joint diameter. This solution is applicable to manipulator products using the same rope-driven joint drive principle as well as self-developed manipulator experimental platforms, enabling the sensing of joint angles in rope-driven agile spatial manipulators. Attached Figure Description

[0046] Figure 1 This is a schematic diagram of the quaternion joint at the wrist of a rope-driven robotic arm.

[0047] Figure 2 This is a schematic diagram of the quaternion joint of the wrist of a rope-driven robotic arm from another direction.

[0048] Figure 3 This is a schematic diagram of the second link of the first branch, the second link of the second branch, and the second link of the third branch in the quaternion joint of the wrist of a rope-driven robotic arm.

[0049] Figure 4 A flowchart for a method to sense the equivalent rotation angle of a quaternion joint at the wrist of a rope-driven robotic arm.

[0050] Figure 5 This is a schematic diagram of the equivalent joint axis of the quaternion joint at the wrist of a rope-driven robotic arm.

[0051] Figure 6 A schematic diagram of the ideal ball rolling motion model of the quaternion joint of the wrist of a rope-driven robotic arm.

[0052] Figure 7 This is a schematic diagram of the DH coordinate system for a single branch of a quaternion joint at the wrist of a rope-driven robotic arm.

[0053] Figure Labels

[0054] 100, Base; 110, First joint encoder; 120, Second joint encoder; 130, First plane; 140, First drive rope seat; 141, First guide seat; 150, Second drive rope seat; 160, Third drive rope seat; 161, Third guide seat; 170, Fourth drive rope seat; 171, Fourth guide seat; 200, First branch; 210, First link of the first branch; 220, Second link of the first branch; 230, Third link of the first branch; 250, First branch seat; 260, Fourth branch seat; 300, Second branch; 310, First link of the second branch; 320, Second link of the second branch; 330, Second link of the second branch... Three-link; 350, second branch seat; 360, fifth branch seat; 400, third branch; 410, first link of the third branch; 420, second link of the third branch; 430, third link of the third branch; 450, third branch seat; 460, sixth branch seat; 500, wrist seat; 510, second plane; 520, fifth drive rope seat; 530, sixth drive rope seat; 540, seventh drive rope seat; 550, eighth drive rope seat. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] The following will provide a clear and complete description of the concept, specific structure, and technical effects of the present invention in conjunction with the embodiments and accompanying drawings, so as to fully understand the purpose, solution, and effects of the present invention.

[0057] It should be noted that, unless otherwise specified, when a feature is referred to as "fixed" or "connected" to another feature, it can be directly fixed or connected to the other feature, or indirectly fixed or connected to the other feature. The singular forms "a," "described," and "the" used herein are also intended to include the plural forms, unless the context clearly indicates otherwise. Furthermore, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used in this specification is for the purpose of describing particular embodiments only and not for limiting the invention. The term "and / or" as used herein includes any combination of one or more of the associated listed items.

[0058] It should be understood that although the terms first, second, third, etc., may be used in this disclosure to describe various elements, these elements should not be limited to these terms. These terms are only used to distinguish elements of the same type from each other. For example, a first element may also be referred to as a second element without departing from the scope of this disclosure, and similarly, a second element may also be referred to as a first element. Any and all instances or exemplary language (“e.g.,” “such as,” etc.) used herein are intended only to better illustrate embodiments of the invention and, unless otherwise required, do not impose a limitation on the scope of the invention. Furthermore, the industry term “pose” as used herein refers to the position and orientation of an element relative to a spatial coordinate system.

[0059] Reference Figures 1 to 7 This invention provides a method for sensing the quaternion joint of the wrist of a cable-driven robotic arm and its equivalent rotation angle, referring to... Figures 1 to 3 The device includes:

[0060] A base 100, wherein a first joint encoder 110 and a second joint encoder 120 are installed in the base 100;

[0061] The first branch 200 is a four-bar linkage, and the first end of the first branch 200 passes through the base 100 and is connected to the first joint encoder 110.

[0062] The second branch 300 is a four-bar linkage, and the first end of the second branch 300 passes through the base 100 and is connected to the second joint encoder 120.

[0063] The third branch 400 is a four-bar linkage, and the first end of the third branch 400 is fixed on the first plane 130 of the base 100.

[0064] The first branch 200, the second branch 300, and the third branch 400 are cyclically symmetrically distributed;

[0065] The wrist base 500 has its second end of the first branch 200, the second end of the second branch 300, and the second end of the third branch 400 connected to the second plane 510 of the wrist base 500.

[0066] Specifically, two of the three four-bar linkages (i.e., the first linkage 200 and the second linkage 300) are connected to the base 100, and joint encoders (i.e., the first joint encoder 110 and the second joint encoder 120) are respectively installed at the rotation angles of the shafts. The position of their shafts is fixed relative to the base 100, which facilitates the installation of the joint encoders. The four-bar linkage structure allows them to be installed at the rear, which greatly reduces the diameter of the wrist joint.

[0067] Furthermore, refer to Figures 1 to 3 The first branch 200 includes a first branch first link 210, a first branch second link 220 and a first branch third link 230 connected in sequence. The first end of the first branch first link 210 is connected to the first joint encoder 110 through a first branch seat 250 disposed on the first plane 130 of the base 100. The second end of the first branch third link 230 is connected to the wrist seat 500 through a fourth branch seat 260 disposed at the bottom of the wrist seat 500.

[0068] The second branch 300 includes a second branch first link 310, a second branch second link 320, and a second branch third link 330 connected in sequence. The first end of the second branch first link 310 is connected to the second joint encoder 120 through a second branch seat 350 disposed on the first plane 130 of the base 100. The second end of the second branch third link 330 is connected to the wrist seat 500 through a fifth branch seat 360 disposed at the bottom of the wrist seat 500.

[0069] The third branch 400 includes a third branch first link 410, a third branch second link 420, and a third branch third link 430 connected in sequence. The first end of the third branch first link 410 is connected to the third branch seat 450 disposed on the first plane 130 of the base 100, and the second end of the third branch third link 430 is connected to the wrist seat 500 through a sixth branch seat 460 disposed at the bottom of the wrist seat 500.

[0070] Furthermore, refer to Figures 1 to 3 The base 100 has four sides on its first plane 130, and each side is provided with a first drive rope seat 140, a second drive rope seat 150, a third drive rope seat 160 and a fourth drive rope seat 170 for connecting the drive rope.

[0071] The second plane 510 of the wrist seat 500 is provided with four sides, and each side is provided with a fifth drive rope seat 520, a sixth drive rope seat 530, a seventh drive rope seat 540 and an eighth drive rope seat 550 for connecting the drive rope.

[0072] The first drive rope seat 140 and the fifth drive rope seat 520 are correspondingly arranged, the second drive rope seat 150 and the sixth drive rope seat 530 are correspondingly arranged, the third drive rope seat 160 and the seventh drive rope seat 540 are correspondingly arranged, and the fourth drive rope seat 170 and the eighth drive rope seat 550 are correspondingly arranged.

[0073] Furthermore, refer to Figures 1 to 3 The bottom of the base 100 also includes a first guide seat 141, a second guide seat, a third guide seat 161 and a fourth guide seat 171. The first guide seat 141, the second guide seat, the third guide seat 161 and the fourth guide seat 171 are all arranged on the same side of the first drive rope seat 140, the second drive rope seat 150, the third drive rope seat 160 and the fourth drive rope seat 170.

[0074] Furthermore, refer to Figures 1 to 3 It also includes a first drive rope, a second drive rope, a third drive rope, and a fourth drive rope, the first ends of which are all pulled out from the drive box;

[0075] The first drive rope passes sequentially through the first pulley of the first guide seat 141, the first pulley of the first drive rope seat 140, the first pulley of the fifth drive rope seat 520, the second pulley of the fifth drive rope seat 520, the second pulley of the first drive rope seat 140, and the second pulley of the first guide seat 141 before returning to the drive box.

[0076] The second drive rope passes sequentially through the first pulley of the second guide seat, the first pulley of the second drive rope seat 150, the first pulley of the sixth drive rope seat 530, the second pulley of the sixth drive rope seat 530, the second pulley of the second drive rope seat 150, and the second pulley of the second guide seat before returning to the drive box.

[0077] The third drive rope passes sequentially through the first pulley of the third guide seat 161, the first pulley of the third drive rope seat 160, the first pulley after the seventh drive rope seat 540, the second pulley after the seventh drive rope seat 540, the second pulley of the third drive rope seat 160, and the second pulley of the third guide seat 161 before returning to the drive box.

[0078] The fourth drive rope passes sequentially through the first pulley of the fourth guide seat 171, the first pulley of the fourth drive rope seat 170, the first pulley after the eighth drive rope seat 550, the second pulley after the eighth drive rope seat 550, the second pulley of the fourth drive rope seat 170, and the second pulley of the fourth guide seat 171 before returning to the drive box.

[0079] Furthermore, refer to Figures 1 to 3 The quaternion joints of the wrist of the rope-driven robotic arm are equivalent to the first rotary joint, the second rotary joint, the first pitch joint, and the second pitch joint.

[0080] The first rotary joint and the second rotary joint have the same rotation angle but opposite directions. The rotation angle of the first rotary joint and the second rotary joint is the azimuth angle.

[0081] The first pitch joint and the second pitch joint have the same rotation angle in both magnitude and direction, and the rotation angle of the first pitch joint and the second pitch joint is the tilt angle.

[0082] Specifically, the wrist is equivalent to two rotational joints with the same magnitude and opposite direction of rotation angles, and two pitch joints with the same magnitude and direction of rotation angles. The motion state of the quaternion joint is represented by these two angles.

[0083] Specifically, refer to Figure 5 and Figure 6 The wrist is equivalent to two rotary joints with the same magnitude but opposite directions of rotation angles, and two pitch joints with the same magnitude and direction of rotation angles. The positions and directions of the rotation axes of the two equivalent rotary joints are shown as z0 and z3, and the positions and directions of the rotation axes of the two pitch joints are shown as z1 and z2. The motion state of the quaternion joints is represented by these two angles. The rotation angle of the rotary joint is the azimuth angle. The pitch angle is the angle of rotation of the pitch joint. .

[0084] Reference Figure 6 The wrist quaternion joint movement manifests as the meshing motion of two hemispheres, driven by four drive ropes distributed in the upper and lower planes. , , , Driven by, the radius of the distribution circle of the rope is Establish a 100 coordinate system on the upper and lower planes respectively. and end coordinate system The equivalent rotational joint angle is the azimuth angle. The position of the wrist flexion plane is determined; the angle of deflection along the flexion plane is the equivalent pitch angle, i.e., the tilt angle. The decision was made.

[0085] Furthermore, refer to Figure 4 The present invention also proposes a method for sensing the equivalent rotation angle of the quaternion joint of the wrist of a cable-driven robotic arm, which is applied to the quaternion joint of the wrist of a cable-driven robotic arm. The method includes the following steps:

[0086] S100: Construct the branch configuration equations of the first branch 200, the second branch 300 and the third branch 400 to obtain the joint kinematic model;

[0087] S200. Determine the orientation angle and tilt angle of the quaternion joint of the robotic arm wrist by using the end position of the quaternion joint of the robotic arm wrist.

[0088] S300. Based on the joint kinematic model, calculate the positions of the second end of the first branch 200, the second end of the second branch 300, and the second end of the third branch 400;

[0089] S400: Calculates end position, azimuth, and tilt angle based on numerical fitting.

[0090] Specifically, the wrist quaternion joint is a joint composed of three links. By measuring the rotation angles of any two links in the quaternion joint, the equivalent motion state of the quaternion joint can be determined. For one of the four-link links, its kinematic model has an analytical expression. If four encoders are installed on one of the four-link links, the motion state of the quaternion joint can be determined in real time using only the forward kinematic model of one link. However, this increases the number of encoders, leading to increased complexity in the mechanical structure and communication wiring, and thus limiting the degrees of freedom of the robotic arm's overall movement. To reduce the number of encoders, a sensing method is proposed that estimates the motion state of the quaternion joint in real time using only the link rotation angles measured by two encoders.

[0091] Furthermore, refer to Figure 4 In step S100,

[0092] The branch configuration equation is:

[0093]

[0094] in, , and respectively Let be the homogeneous transformation matrix of the coordinate systems of the ends of the first branch 200, the second branch 300, and the third branch 400 relative to the coordinate system of the base 100. , , These represent the circumferential phase distributions of the first branch 200, the second branch 300, and the third branch 400 relative to the first plane 130. These are the angles between adjacent links of the first branch 200, namely the angle between the first branch seat 250 and the first link 210 of the first branch, the angle between the first link 210 and the second link 220 of the first branch, the angle between the second link 220 and the third link 230 of the first branch, and the angle between the third link 230 and the fourth branch seat 260; These are the angles between adjacent links of the second branch 300, namely the angle between the second branch seat 350 and the first link 310 of the second branch, the angle between the first link 310 of the second branch and the second link 320 of the second branch, the angle between the second link 320 of the second branch and the third link 330 of the second branch, and the angle between the third link 330 of the second branch and the fifth branch seat 360; These are the angles between adjacent links of the third branch 400, namely the angle between the third branch seat 450 and the first link 410 of the third branch, the angle between the first link 410 of the third branch and the second link 420 of the third branch, the angle between the second link 420 of the third branch and the third link 430 of the third branch, and the angle between the third link 430 of the third branch and the sixth branch seat 460.

[0095] Specifically, the three four-bar linkages are cyclically symmetrically distributed in space. Due to the parallel constraints between the rotation angles of each linkage, i.e., the end poses of the three four-bar linkages are the same, the following set of nonlinear equations can be obtained, thus yielding the joint kinematic model, where θ1a represents the circumferential phase distribution of the four-bar linkage.

[0096] The three four-link chains of this joint are cyclically symmetrically distributed in space. Due to the parallel constraints between the rotation angles of each chain, the end poses of the three four-link chains are identical. The homogeneous transformation matrices of the end coordinate systems of the first chain 200, the second chain 300, and the third chain 400 relative to the coordinate system of the base 100 are respectively... , , The following set of nonlinear equations can be obtained, thus yielding the joint kinematic model. Coordinate systems are established for the first branch 200, the second branch 300, and the third branch 400 using the standard DH coordinate system establishment method. , , These represent the circumferential distribution phases of branches a, b, and c, respectively. , , These are the rotation angles between adjacent links of the first branch 200, the second branch 300, and the third branch 400, respectively. These angles determine the homogeneous transformation matrix between adjacent links and the configuration of this four-bar linkage.

[0097]

[0098] Where θ2a, θ3a, θ4a, and θ5a are the rotation angles between adjacent links, which determine the homogeneous transformation matrix between adjacent links.

[0099] Furthermore, refer to Figure 4 In step S200, the orientation angle and tilt angle of the quaternion joint of the wrist of the cable-driven robotic arm are:

[0100]

[0101] Where φ is the orientation angle of the quaternion joint, θ is the tilt angle of the quaternion joint, xe is the x-component of the quaternion joint end position, ye is the y-component of the quaternion joint end position, and ze is the z-component of the quaternion joint end position.

[0102] Specifically, the movement between the upper and lower discs of the joint is equivalent to pure rolling motion with point contact between two spherical surfaces. Therefore, the distance from the center Oe of the upper spherical surface (the center of the second plane 510) to the center O0 of the lower spherical surface (the center of the first plane 130) is a constant, and its spatial trajectory is also on a spherical surface. Therefore, its azimuth angle φ and tilt angle θ can be uniquely determined by the end position [xe,ye,ze].

[0103] Furthermore, refer to Figure 4 In step S300,

[0104] The end position of the second end of the first branch 200 is:

[0105]

[0106] Where pae is the end position of the second end of the first branch 200. The distance from the center of the plane to the connection point between the first branch chain seat 250 and the fourth branch chain seat 260 and the upper and lower planes. The distance between the common perpendicular line of the first branch chain seat 250 pivot and the first link 210 pivot of the first branch chain.

[0107] The relationship between the equivalent joint angle of the wrist and the distal end position is obtained. If the relationship between the distal end position and the rotation angle of the first branch 200 is obtained, the rotation angle of the link is the angle measured by the joint encoder. Therefore, let's analyze the first branch 200. Through the forward kinematic model of a single-branch four-bar linkage, we can know that the distal end position of the branch... Only The function, and Irrelevant The distance from the center of the plane to the connection point between the first link 210 and the third link 230 of the first branch and the upper and lower planes. The distance between the common perpendicular lines of the first link 210 pivot of the first branch and the second link 220 pivot of the first branch.

[0108] Specifically, the relationship between the equivalent joint angle of the wrist and the end position is obtained. If the relationship between the end position and the rotation angle of the branch link is obtained, the rotation angle of the link link is the angle measured by the joint encoder. Therefore, let's take the first branch for analysis. Through the forward kinematic model of a single branch four-bar linkage, it can be seen that the end position pae of the branch is only a function of θ2a, θ3a, and θ4a, and is independent of θ5a.

[0109] Based on the solution results of the nonlinear equations determined by the constraints, the corresponding relationships between θ2a, θ3a, and θ4a can be obtained. Therefore, θ4a can be calculated using numerical fitting based on the values ​​of θ2a and θ3a, and then the end position pae, as well as the azimuth angle φ and inclination angle θ can be calculated.

[0110] Reference Figure 7 Taking the first branch 200 as an example, coordinate systems are established for the first branch 200, the second branch 300, and the third branch 400 respectively according to the standard DH coordinate system establishment method. The base 100 coordinate system 0 is established at the center of the first plane 130 of the base 100. , To bypass The rotation angle represents the circumferential phase distribution of the first branch 200. Coordinate systems are established sequentially with the rotation axes of the four links of the first branch 200 as the z-axis. , , , , It is the rotation angle between the adjacent link of branch a and the z-axis. (5-series) Relative translation distance of the 4 series This makes the origin of the coordinate system coincide with the center of the upper plane, and the end coordinate system It overlaps with the 5 series.

[0111] Compared with existing technologies, the present invention has the following characteristics:

[0112] This invention provides a method for sensing the equivalent rotation angle of the wrist quaternion joint in a rope-driven agile spatial manipulator. This ensures that the rope-driven agile spatial manipulator can obtain feedback on the wrist joint position during motion planning and control, and that this feedback corresponds to the joint angle motion planning control parameters, creating the prerequisite for achieving closed-loop joint control of the manipulator's wrist joint. The encoder is mounted on a branch shaft connected to the base 100, with the shaft position fixed relative to the base 100, facilitating rear-mounting and significantly reducing the wrist joint diameter. This solution is applicable to manipulator products using the same rope-driven joint drive principle as well as self-developed manipulator experimental platforms, enabling the sensing of joint angles in rope-driven agile spatial manipulators.

[0113] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention, as long as they achieve the technical effects of the present invention by the same means, should be included within the scope of protection of the present invention. Within the scope of protection of the present invention, the technical solutions and / or implementation methods can have various modifications and variations.

Claims

1. A quaternion joint for the wrist of a cable-driven robotic arm, characterized in that, include: A base (100) is provided, wherein a first joint encoder (110) and a second joint encoder (120) are installed in the base (100). The first branch (200) is a four-bar linkage, and the first end of the first branch (200) passes through the base (100) and is connected to the first joint encoder (110). The second branch (300) is a four-bar linkage, and the first end of the second branch (300) passes through the base (100) and is connected to the second joint encoder (120). The third branch (400) is a four-bar linkage, and the first end of the third branch (400) is fixed on the first plane (130) of the base (100); The first branch (200), the second branch (300) and the third branch (400) are cyclically symmetrically distributed. Two of the three four-bar branches, namely the first branch (200) and the second branch (300), respectively enable the first joint encoder (110) and the second joint encoder (120) to be installed in the rear through the four-bar structure. The wrist base, the second end of the first branch (200), the second end of the second branch (300) and the second end of the third branch (400) are all connected to the second plane (510) of the wrist base (500); The quaternion joints of the wrist of the cable-driven robotic arm are equivalent to the first rotary joint, the second rotary joint, the first pitch joint, and the second pitch joint. The first rotary joint and the second rotary joint have the same rotation angle but opposite directions. The rotation angle of the first rotary joint and the second rotary joint is the azimuth angle. The rotation angles of the first pitch joint and the second pitch joint are the same in magnitude and direction, and the rotation angles of the first pitch joint and the second pitch joint are the tilt angles. The wrist quaternion joint movement manifests as the meshing motion of two hemispheres, driven by four drive ropes distributed in the upper and lower planes, namely the first drive rope. Second drive rope Third drive rope and the fourth drive rope Driven by, the radius of the distribution circle of the rope is Establish a base (100) coordinate system on the upper and lower planes respectively. and end coordinate system The equivalent rotation joint angle is the azimuth angle. The position of the wrist flexion plane is determined; the angle of deflection along the flexion plane is the equivalent pitch angle, i.e., the tilt angle. The decision was made.

2. The quaternion joint of the wrist of the cable-driven robotic arm according to claim 1, characterized in that, The first branch (200) includes a first branch first link (210), a first branch second link (220) and a first branch third link (230) connected in sequence. The first end of the first branch first link (210) is connected to the first joint encoder (110) through a first branch seat (250) disposed on the first plane (130) of the base (100). The second end of the first branch third link (230) is connected to the wrist seat (500) through a fourth branch seat (260) disposed at the bottom of the wrist seat (500). The second branch (300) includes a second branch first link (310), a second branch second link (320), and a second branch third link (330) connected in sequence. The first end of the second branch first link (310) is connected to the second joint encoder (120) through a second branch seat (350) disposed on the first plane (130) of the base (100). The second end of the second branch third link (330) is connected to the wrist seat (500) through a fifth branch seat (360) disposed at the bottom of the wrist seat (500). The third branch (400) includes a third branch first link (410), a third branch second link (420) and a third branch third link (430) connected in sequence. The first end of the third branch first link (410) is connected to the third branch seat (450) disposed on the first plane (130) of the base (100). The second end of the third branch third link (430) is connected to the wrist seat (500) through a sixth branch seat (460) disposed at the bottom of the wrist seat (500).

3. The quaternion joint of the wrist of the cable-driven robotic arm according to claim 1, characterized in that, The base (100) has four sides on its first plane (130), and each side is provided with a first drive rope seat (140), a second drive rope seat (150), a third drive rope seat (160) and a fourth drive rope seat (170) for connecting the drive rope. The second plane (510) of the wrist seat (500) is provided with four sides, and each side is provided with a fifth drive rope seat (520), a sixth drive rope seat (530), a seventh drive rope seat (540) and an eighth drive rope seat (550) for connecting the drive rope. The first drive rope seat (140) and the fifth drive rope seat (520) are correspondingly arranged, the second drive rope seat (150) and the sixth drive rope seat (530) are correspondingly arranged, the third drive rope seat (160) and the seventh drive rope seat (540) are correspondingly arranged, and the fourth drive rope seat (170) and the eighth drive rope seat (550) are correspondingly arranged.

4. The quaternion joint of the wrist of the cable-driven robotic arm according to claim 3, characterized in that, The bottom of the base (100) also includes a first guide seat (141), a second guide seat, a third guide seat (161), and a fourth guide seat (171). The first guide seat (141), the second guide seat, the third guide seat (161), and the fourth guide seat (171) are all positioned opposite each other on the same side of the first drive rope seat (140), the second drive rope seat (150), the third drive rope seat (160), and the fourth drive rope seat (170).

5. The quaternion joint of the wrist of the cable-driven robotic arm according to claim 4, characterized in that, It also includes a first drive rope, a second drive rope, a third drive rope, and a fourth drive rope, the first ends of which are all pulled out from the drive box; The first drive rope passes sequentially through the first pulley of the first guide seat (141), the first pulley of the first drive rope seat (140), the first pulley of the fifth drive rope seat (520), the second pulley of the fifth drive rope seat (520), the second pulley of the first drive rope seat (140), and the second pulley of the first guide seat (141) before returning to the drive box; The second drive rope passes sequentially through the first pulley of the second guide seat, the first pulley of the second drive rope seat (150), the first pulley of the sixth drive rope seat (530), the second pulley of the sixth drive rope seat (530), the second pulley of the second drive rope seat (150), and the second pulley of the second guide seat before returning to the drive box; The third drive rope passes sequentially through the first pulley of the third guide seat (161), the first pulley of the third drive rope seat (160), the first pulley of the seventh drive rope seat (540), the second pulley of the seventh drive rope seat (540), the second pulley of the third drive rope seat (160), and the second pulley of the third guide seat (161) before returning to the drive box. The fourth drive rope passes sequentially through the first pulley of the fourth guide seat (171), the first pulley of the fourth drive rope seat (170), the first pulley of the eighth drive rope seat (550), the second pulley of the eighth drive rope seat (550), the second pulley of the fourth drive rope seat (170), and the second pulley of the fourth guide seat (171) before returning to the drive box.

6. A method for sensing the equivalent rotation angle of a quaternion joint in the wrist of a cable-driven robotic arm, applied to the quaternion joint in the wrist of a cable-driven robotic arm as described in any one of claims 1 to 5, characterized in that, The method includes the following steps: S100. Construct the branch configuration equations of the first branch (200), the second branch (300) and the third branch (400) to obtain the joint kinematic model; S200. Determine the azimuth and tilt angles of the quaternion joints at the wrist of the robotic arm by measuring the end position of the quaternion joints at the wrist of the robotic arm. S300. Based on the joint kinematic model, calculate the positions of the second end of the first branch (200), the second end of the second branch (300), and the second end of the third branch (400); S400: Calculates end position, azimuth, and tilt angle based on numerical fitting.

7. The method for sensing the equivalent rotation angle of the quaternion joint of the wrist of a cable-driven robotic arm according to claim 6, characterized in that, In step S100, The branch configuration equation is: ， in, , and respectively Let be the homogeneous transformation matrix of the coordinate systems of the ends of the first branch (200), the second branch (300), and the third branch (400) relative to the coordinate system of the base (100). , , These are the circumferential phase distributions of the first branch (200), the second branch (300), and the third branch (400) relative to the first plane (130), respectively. These are the angles between adjacent links of the first branch (200), namely the angle between the first branch seat (250) and the first link (210) of the first branch, the angle between the first link (210) of the first branch and the second link (220) of the first branch, the angle between the second link (220) of the first branch and the third link (230) of the first branch, and the angle between the third link (230) of the first branch and the fourth branch seat (260); These are the angles between adjacent links of the second branch (300), namely the angle between the second branch seat (350) and the first link (310) of the second branch, the angle between the first link (310) of the second branch and the second link (320) of the second branch, the angle between the second link (320) of the second branch and the third link (330) of the second branch, and the angle between the third link (330) of the second branch and the fifth branch seat (360); These are the angles between adjacent links of the third branch (400), namely the angle between the third branch seat (450) and the first link (410) of the third branch, the angle between the first link (410) of the third branch and the second link (420) of the third branch, the angle between the second link (420) of the third branch and the third link (430) of the third branch, and the angle between the third link (430) of the third branch and the sixth branch seat (460).

8. The method for sensing the equivalent rotation angle of the quaternion joint of the wrist of a rope-driven robotic arm according to claim 6, characterized in that, In step S200, the azimuth and tilt angles of the quaternion joints of the wrist of the cable-driven robotic arm are: ， in, The azimuth angle of the quaternion joint. The tilt angle of the quaternion joint. Let x be the x-component of the quaternion joint end position. Let be the y-component of the quaternion joint end position. The component of the quaternion joint end position in the z-direction.

9. The method for sensing the equivalent rotation angle of the quaternion joint of the wrist of a rope-driven robotic arm according to claim 6, characterized in that, In step S300 The end position of the second end of the first branch (200) is: ， Where Pae is the end position of the second end of the first branch (200), The distance from the center of the plane to the connection point between the first branch chain seat (250) and the fourth branch chain seat (260) and the upper and lower planes. The distance between the common perpendicular line of the first branch chain seat (250) pivot and the first branch chain first link (210) pivot.

Images