Dynamic Modeling of Three-Degree-of-Freedom Hydraulic Wrist for Mine

: Hydraulic manipulator is a kind of high precision mechanical device, which plays an important role in industrial production. Wrist structure is an indispensable part of common hydraulic manipulator, and its performance directly determines the overall working condition and performance of robot actuators. Aiming at the hydraulic wrist system, the kinematics modeling is carried out by using the space coordinate transformation theory and the robot modeling theory. The Jacobian matrix of the robot and the Jacobian matrix of the robot are solved. According to the rigid body dynamics principle of the serial manipulator and the Lagrange method, the dynamic model of the hydraulic wrist is established. The Newton-Euler method is suitable for the modeling of the traditional connecting rod manipulator. The disadvantage of the Lagrange method is that the calculation amount is large when modeling the multi-degree-of-freedom system. The object of this paper is the hydraulic wrist, so the Lagrange method is used to model the wrist.


Introduction
The hydraulic manipulator [1] is a mechanical device with high precision, high reliability, high performance and high efficiency.It can realize complex work such as positioning, handling and welding, and plays an important role in industrial production.In order to make the end-effector of the robot arm have the flexibility similar to that of the human hand, it is necessary to develop a unit similar to the human wrist to realize the intermediate transition between the robot end-effector and the robot arm [2][3].The traditional Pitch-yaw-roll [4][5][6] wrist is not suitable for working in a narrow space due to its complex structure, multi-stage transmission links, lack of tightness in the overall structure, occupying a large amount of space, and high assembly accuracy requirements of the whole machine.Although the wrist based on spherical gear transmission, which uses the motion mechanism of spherical gear pair to realize the swing and rotation of wrist, is compact, the machining accuracy of this kind of wrist is strict and the manufacturing cost is high, so it is limited in practical application [7][8].The wrist based on the parallel mechanism can solve the space occupation problem of the traditional wrist to a certain extent.However, when the inverse kinematics solution is carried out, sometimes it is impossible to solve the effective closed solution.
At this time, it will bring difficulties to the pose control of the robot, making it difficult for the robot to achieve accurate motion control [9][10].
Due to the large space occupied by the traditional hydraulic manipulator, the difficulty of control is high, and the power loss is large.In order to solve the demand of the explosion-proof full hydraulic manipulator in coal mine and the above problems, this paper develops a three-degree-of-freedom spherical wrist based on hydraulic drive technology with small shape, compact structure, short transmission chain, strong attitude ability, high integration, high power density and motion decoupling to replace the electric drive wrist.Compared with the motor drive scheme, the hydraulic drive scheme has obvious advantages in the transmission system.Since the hydraulic drive scheme does not require the transmission system, the power-to-weight ratio and space utilization rate of the spherical wrist can be significantly improved.The hydraulic wrist consists of five parts, including pump source unit, valve system (composed of solenoid valves), hydraulic wrist, hydraulic cylinder system (including linear single-out-bar hydraulic cylinder and rotary hydraulic cylinder), data acquisition system and computer control system.

The Composition of the Wrist Mechanism
The focus of this research is the three-degree-of-freedom hydraulic wrist.The hydraulic wrist consists of five parts, including the pump source unit, the valve system (composed of solenoid valves), the hydraulic wrist, the hydraulic cylinder system (including the linear single bar hydraulic cylinder and the rotary hydraulic cylinder), the data acquisition system and the computer control system [11][12].
The movement of the wrist output end includes three degrees of freedom of pitch, side swing and rotation.These three degrees of freedom are decoupled, independent of each other and have no influence on each other.  1 shows the structure of the wrist, which is composed of: 1-hydraulic motor; 2-pitch cylinder; 3-pitch connecting plate 2; 4-pitching component, which is driven by 2 to achieve pitch rotation; 5-constant velocity universal joint; 6-pitch angle sensor; 7-side swing connecting plate 2; 8-rotation angle encoder; 9-end connection flange; 10-end cover; 11-swing connecting plate one; 12-limit block; 13-side-swing angle sensor; 14-side-swing cylinder; 15-pitch swing component; 16-auxiliary ribs; 17-pitch connecting plate one; 18-coupling; 19-The floor.

The Working Principle of the Wrist
(1) Working principle of side swing joint The swing joint is the first joint in the wrist, and its drive system adopts a valve-controlled asymmetric cylinder electro-hydraulic servo control system.The working principle of the system is to control the extension and retraction of the piston rod of the side swing hydraulic cylinder through the servo valve, so that the end cover drives the output shaft of the universal joint and the side swing connecting plate to rotate around the side swing axis to one side or the other side to realize the side swing motion.Specifically, under the control of the servo valve, when the piston rod of the side swing hydraulic cylinder extends outward, the piston rod will first push the end cover, and then the end cover drives the universal joint output shaft and the side swing connection plate to rotate in one direction around the side swing rotation axis ; when the piston rod of the side swing hydraulic cylinder is controlled by the servo valve to retract inward, it will first pull the end cover and drive the universal joint output shaft and the side swing connecting plate to rotate in another direction around the side swing rotation axis.Therefore, relying on the movement of the piston rod of the asymmetric cylinder, the swing angle range of the side swing joint can reach positive and negative 30°.
(2) Working principle of pitch joint The wrist of an industrial robot is usually composed of two joints, of which the pitch joint is the second joint.The drive system of the pitch joint adopts the valve-controlled asymmetric cylinder electro-hydraulic servo control system.The pitch motion is realized by controlling the expansion of the piston rod of the asymmetric pitch cylinder, and the swing angle range is positive and negative 35 degrees.The motion of the pitch joint affects the swing joint, because the swing joint is equivalent to the load of the pitch joint, and its working principle is similar to that of the swing joint.The specific working principle is realized by the extension and retraction of the piston rod of the pitch cylinder controlled by the servo valve.The servo valve controls the piston rod of the pitch cylinder to expand and contract, prompting the pitch component to rotate and driving the side swing joint to rotate along the pitch rotation axis to both sides.The extension and retraction of the piston rod cause the same-direction and reverse rotation of the pitch component and the side swing joint, respectively, so as to realize the precise pitch motion of the robot.Therefore, this method can achieve pitch motion by controlling the movement of the piston rod.At the same time, the pitch rotation axis and the side swing rotation axis intersect perpendicularly at the intersection of the wrist center, thus ensuring the precise control of the robot arm.
(3) Working principle of self-rotation joint The rotation joint is an important part of the robot.The valve-controlled motor and electro-hydraulic servo control system are used in its drive system to achieve efficient control and movement.In the drive system, the hydraulic motor can achieve full-cycle rotation.The output shaft of the hydraulic motor is connected to the input shaft of the constant-speed ball cage universal joint by a coupling, and the output shaft of the universal joint is connected to the end cover through a bearing.The connection between the end cover and the end is carried out by flange, and the end cover and the flange are also equipped with an autorotation angle encoder.The rotary motor is a device that realizes automatic rotation, and its motion control needs to be adjusted by servo valve.The torque controlled by the servo valve is transmitted to the output end of the wrist through the universal joint to realize the rotation motion and ensure the speed synchronization of the hydraulic motor.In this process, the constant velocity universal joint is used to ensure that the output speed of the wrist end is completely synchronized with the speed of the driving hydraulic motor.Therefore, using the hydraulic system to drive the rotary motor can achieve stable and reliable rotary motion.

Kinematics Modeling of Wrist
In the hydraulic wrist described in this paper, the kinematic relationship is the influence of the hydraulic cylinder expansion on the end of the wrist.The wrist studied in this paper has three degrees of freedom: pitch, side swing, and rotation.Because the rotation of the wrist does not affect the end position of the wrist, the kinematic relationship of the wrist only needs to be pitched, and the relationship between the extended displacement  1 ,  2 of the side swing hydraulic cylinder and the end position P of the wrist can be (formula 3).Firstly, the relationship between the pitch angle , the side swing angle  and the position of the wrist end (formula 1) is established, and then the relationship between the pitch and side swing hydraulic cylinder extension displacement  1 ,  2 and the pitch angle , the side swing angle  of the hydraulic cylinder (formula 2 and 3) is obtained.The relationship between the pitch, the side swing hydraulic cylinder extension displacement  1 ,  2 and the position P of the wrist end can be obtained immediately by combining the two.The specific steps are as follows: Firstly, the fixed coordinate system 0 is selected to discuss the wrist modeling problem.In order to facilitate the calculation, the rotation axis of the wrist pitching action and the rotation axis of the side swing action are selected as the origin of the coordinates.All the discussions after this paper are based on this coordinate.The coordinate system 0 is shown in Figure 2: In this paper, the posture of the end coordinate system of the wrist is represented by the Z-Y-Z Euler angle.Assuming that the distance between the end of the wrist and the origin of the coordinate is  0 , the position P0 of the end of the wrist in the coordinate system 0 can be expressed as (0,0, 0 ).Assuming that the wrist pitch angle is  and the side swing angle is , then the coordinate of the end position P can be expressed by the Euler angle method with the rotation matrix at 0: The end coordinates of the wrist can be simplified as: The formula 2 gives the expression of the end position of the wrist P at 0 with the wrist pitch angle  and the side swing angle .The above formula can also be used to know the coordinates of the end of the wrist and reverse the wrist pitch and side swing angle.From Fig. 3, the relationship between the pitch angle  and the pitch cylinder extension distance  1 is as follows: Among them, O 1 , O 2 , O 3 are the rotational joints between the cylinder body of the pitching cylinder and the pitching auxiliary support, the rotational joints between the pitching component and the pitching connecting plate, and the rotational joints between the piston cylinder of the pitching cylinder and the pitching component, respectively.O 1 A, O 2 B are the vertical distance between the rotational joint and the bottom plate (the value is e 1 ), the vertical distance between the rotational joint O 2 and the bottom plate (the value is b 1 ), AB is the vertical distance between the rotational joint O 1 , O 2 and the bottom plate (the value is a 1 ), c 1 is the distance between the rotational joint O 2 and the rotational joint O 3 .l 1 is the distance between the rotation joint O 1 and the rotation joint O 3 when the pitch link mechanism is in the limit position ( = −35°, the horizontal direction of the pitch link is zero, and the clockwise direction is positive). 1 is the output displacement of the translational joint of the piston rod of the pitch cylinder. represents the angle of the wrist's pitch action, and the horizontal direction is zero and the clockwise direction is positive.
a +b +c +e -l + a +b +c +e -l arccos -arccos 2 a +e b +c 2 a +e b +c ), l 2 is the distance between the rotating joint O 5 and the rotating joint O 6 when the side swing link mechanism is in the limit position ( = −30, the vertical direction of the side swing link is zero, and the counterclockwise direction is positive),  2 is the output displacement of the piston rod translational joint of the side swing cylinder,  is the side swing angle of the wrist (the vertical direction is zero, and the counterclockwise direction is positive).
Substituting (formula 3) (formula 4) into (formula 2), the forward kinematics model of the wrist is obtained.That is, in the coordinate system 0, the end coordinate P of the wrist is expressed by variables  1 and  2 in the three-dimensional space with O as the origin:

         
In formula 5, x 1 represents the output displacement of the piston rod translational joint of the pitch cylinder, x 2 represents the output displacement of the piston rod translational joint of the side swing cylinder, and the remaining, a 1 , b 1 , c 1 , e 1 , l 1 , a 2 , b 2 , c 2 , e 2 , l 2 are constants.The position relationship between the wrist joint space and the driving space is established in formula 5.
On this basis, the velocity coordinates of the end of the wrist expressed by the pitch and side swing angles  and  can be obtained by deriving the time on both sides of (formula 5) at the same time: 0 0 0 0 00 cos 0 cos cos cos sin sin cos cos sin sin sin cos cos sin cos cos sin sin Among them, VX, VY and VZ are the components of the velocity vector of the wrist end P in the fixed coordinate system 0 along the x, y and z axes.In the following discussion, the relationship between the speed of the wrist end and the extension speed of the pitch cylinder ( 1 ̇), the extension speed of the side swing cylinder ( 2 ̇) can be obtained by deriving the formula 5 to time: The Jacobian matrix of the wrist can be obtained by expressing the above results in the form of a matrix: In the above formula, J(x) is a matrix of 3 rows and 2 columns: a +b +c +e -l + a +b +c +e -l arccos -arccos 2 a +e b +c 2 a +e b +c

Wrist Dynamics Modeling
Firstly, the generalized coordinates are selected, as shown in Fig. 5.The red frame driven by the pitching cylinder of the wrist is the first part, and the part driven by the side swing cylinder is the second part.Ixx1 is the moment of inertia of the first part to the x-axis, Ixx2 is the moment of inertia of the second part to the x-axis, Iyy1 is the moment of inertia of the first part to the y-axis, and Iyy2 is the moment of inertia of the second part to the y-axis.α is the angular velocity of the first part, β is the angular velocity of the second part.The Lagrangian dynamic equation is a method of deriving the dynamic equation from the scalar function.This scalar function is called the Lagrangian function, which describes the difference between the total kinetic energy and the potential energy of the manipulator.In this paper, the Lagrangian function is defined as follows: The Lagrange equation of the conventional manipulator is d L L dt q q     ,  is the torque output by the motor that controls the manipulator.In this hydraulic wrist system, since the drive is the linear displacement output by the hydraulic cylinder, the traditional sense of  is expressed as the product of the hydraulic cylinder output force F and the force arm in this system.In the subsystem driven by the pitch hydraulic cylinder, F 1 sin 1 • c 1 is used to "replace" , where c1 is a constant as mentioned above,  1 is a constantly changing angle, and  1 can be expressed as: In the subsystem driven by a side-swing hydraulic cylinder, F 2 sin 2 • c 2 is used to "replace" , where c2 is a constant as mentioned above,  2 is a changing angle,  2 can be expressed as: The q in the traditional sense is the displacement  1 and  2 of the hydraulic cylinder output in this system, so the Lagrange equation of the wrist can be expressed as: Because the motion of the first part is fixed axis rotation, the kinetic energy of the first part should be expressed as: The second part is the superposition of translation and fixed axis rotation, so the kinetic energy should be expressed as: In formula 14, since the center of mass of the second part and the end of the wrist are on the same straight line passing through the origin,  2 and   are linear, and the proportional coefficient is k2.Therefore, the total kinetic energy K is expressed as: The first part of the potential energy can be expressed as: Combining formula 1 and formula 18, the dynamic equation of wrist is: ( ) ( , ) ( ) ( ) M q q C q q q G q J F d t In the formula,  is the output torque of the joint actuator, c is the joint damping torque,   () is the unmodeled term and the external disturbance, J T is the transpose of the wrist Jacobian matrix, and   is the load at the end of the wrist.

Figure 2 :
Figure 2: Coordinate system of the wrist 0

Figure 3 :
Figure 3: Structure chart of pitching joint of hydraulic wrist

Figure 4 :
Figure 4: Structure chart of swing joint of hydraulic wristFrom Figure4, the relationship between the yaw angle  and the yaw cylinder extension distance  2 is as follows:

Among them, O 4
is the rotation joint between the side swing connecting plate 1 and the pitching component, O 5 is the rotation joint between the cylinder body of the side swing cylinder and the side swing auxiliary roof, O 6 is the rotation joint between the piston cylinder of the side swing cylinder and the side swing end cover, O 5 C is the vertical distance between the rotation joint O 5 and the wrist pitching axis (value e 2 ), O 6 D is the vertical distance between the rotation joint O 6 and the wrist rotation axis (value C 2 ), O 4 D is the distance between the rotation joint O 4 and the vertical direction of the rotation joint O 6 (value b 2 ).O 4 C is the distance between the rotating joint O 4 and the rotating joint O 5 along the axis of the wrist 's pitching motion (value a 2 a + b -e 2c a + b -e

Figure 5 :
Figure 5: Dynamic modeling diagram of hydraulic wrist