Kinetostatics of a serial robot in screw form

Abstract

Kinetostatics of a robot is an essential component of robotic analysis that includes actuation distribution and torque control. The extant methods mainly include the static transfer formulae and force Jacobian matrix algorithms based on the Denavit-Hartenberg (D-H) method. They can calculate the joint driving torque, but there are some limitations. This paper proposes a kinetostatics approach for analyzing the driving torque of serial manipulators. The instantaneous work done by the external load is expressed with reciprocal screw. It is the key idea of the approach to derive the kinetostatics equation in screw form and determine the required torque at each joint. This methodology is straightforward for programming. Through kinetostatic analysis of some typical serial manipulators, this paper shows the process of establishing the kinetostatics of every joint to calculate the torque in Plücker coordinates. The general spatial manipulator was analyzed and the solution demonstrates the universality of the method presented in this paper. In addition, the kinetostatic analysis method applies to all kinds of serial manipulators.

Introduction

Kinetostatic analysis of a robot is essential for actuation distribution, load balance, and precise control. It is the kinetostatics of mechanism to solve the wrench of joint space when the mechanism remains at rest or slow-motion state. When the serial robot interacts with the environment through the end effector, forces and moments will be generated at the contact position. For example, when extracting heavy objects, the end effector needs to balance the external load. When machining parts, the end effector needs to bear the force from the workpiece. In general, the joint force of the serial manipulator is generated by a separate actuation and transmitted to the end effector through the links to balance the external load. For a mechanism, the link and the joint are subjected to various forces. However, when the mechanism remains at rest or moves slowly, the influence of inertia force and other dynamic effects can be ignored, and only the static effect needs to be considered. In engineering, we hope to find out the feasible joint force and torque under the conditions of maintaining the static equilibrium of the system. Therefore, the relationship between static force and motion in joint space and end effector space plays a vital role in torque control of the robotic arm.

Therefore, the robot kinetostatics is an important research topic, and the existing methods can be found from some literatures.^1,2 There are two main methods, one is based on the Static transfer formula, and the other is the Force Jacobian matrix algorithm based on the D-H method in one Cartesian coordinate system.

The serial robot is an open-chain mechanism composed of a series of links. Without considering the dynamic effect, the forces and moments in the joint space are caused by the forces and moments in the end effector space. From the perspective of Newton mechanics, the static transfer formula is derived from the end link to the primary link. ³ Similarly, Li et al. ⁴ investigated the statics of a 3-DOF robot for Milling based on this method. Juan and Mirats Tur ⁵ reviewed the basic problems of static analysis. Fischer ⁶ studied the statics of cylindrical joints by separating forces and moments. Lee et al. ⁷ calculated the parameter of linkage required to achieve static equilibrium within the static torque range In addition, some static balancing methods about spherical joint mechanisms, ⁸ planar linkage mechanisms,^9,10 Tensegrity mechanisms, ¹¹ and multi-link suspension ¹² have also been developed. This method is widely used in different fields.^13–16 The second method is the force Jacobian matrix approach based on the D-H convention. In a Cartesian coordinate system, work is the dot product of the force vector and displacement vector. From the perspective of work, the static equation of the serial robot in Cartesian coordinate system is derived. ² It is worth noting that the core of this method is to solve the velocity Jacobian matrix based on the D-H method. On this basis, Han et al. ¹⁷ proposed a static model for serial articulated manipulators based on the analysis with standard D-H convention. Kim et al. ¹⁸ analyzed a planar double-wishbone suspension mechanism with the new Jacobian method. This method is also often adopted to analyze the kinematics and statics of the manipulator.^19–21

In the aforementioned research, seldom were the kinetostatics discussed in screw coordinates in such a way. The main contributions of this paper are therefore as follows: (1) The proposed method is very concise in calculating the joint force of a serial robot based on the reciprocal screw. Statics calculation is simpler since it requires only the external load exerting on the end effector and the twist matrix of velocity for the manipulator, and does not need disassembling every joint to study the force for all joints. The calculation of the twist matrix is more convenient than that of the Jacobian matrix. (2) Different from the traditional statics method, the kinetostatics equation of a serial robot is derived in screw form. The instantaneous work done by the external load is expressed with reciprocal screw theory and the kinetostatics equation is derived under the condition of zero virtual power. (3) The real-time output of the manipulator is realized by a numerical algorithm, which is convenient for computer programming. In this study, computer programming that constructs equations of equilibrium for the studied mechanism is extended to analyze the kinetostatics of manipulators. (4) By describing the motion and force in terms of twist and wrench, a unified description method can be established in one coordinate frame, which makes the mechanism analysis more straightforward.

Screw kinetostatics

For the prescribed inputs of actuator velocity $\omega ={\left[\begin{array}{cccc}{\omega }_1\hfill & {\omega }_2\hfill & \cdots \hfill & {\omega }_n\hfill \end{array}\right]}^T\in R^{n\times 1}(1<n\le 6)$ of an n-DOF serial manipulator, it is the forward velocity analysis to find the end effector twist $\xi \in R^{6\times 1}$ . The forward velocity relation can be written as^22,23

$$\xi =S\omega ,$$

(1)

where S is called the twist matrix. Twist matrix can be expressed as

$$S=\left[\begin{array}{ccccc}{\$}_1\hfill & {\$}_2\hfill & \cdots \hfill & {\$}_{n-1}\hfill & {\$}_n\hfill \end{array}\right],$$

(2)

in which ${\$}_i$ is called the twist. The screw ${\$}_i(i=1,2,\cdots ,6)$ is commonly represented by

$${\$}_i=\left[\begin{array}{c}{s}_i\hfill \\ \multicolumn{1}{c}{r_i\times s_i+p{s}_i}\end{array}\right](i=1,2,\cdots ,6),$$

(3)

where p represents the pitch of a screw.

Note that a rotational screw, which is commonly used to represent rotational joints, can be obtained by setting the pitch $p=0$ in equation (3), resulting in

$${\$}_i=\left[\begin{array}{c}{s}_i\hfill \\ \multicolumn{1}{c}{r_i\times s_i}\end{array}\right](i=1,2,\cdots ,6)$$

(4)

where vector $s_i$ indicates the axis vector of ${\$}_i$ and $r_i$ is any vector drawn from the centroid of the Plücker coordinate reference frame to any point of the axis of ${\$}_i$ .

Equation (1) expresses the twist of the serial manipulator. The twist matrix is formed by the unit screws of the rigid body, and it can be programmed on a computer for general calculation.

Similar to kinematic analysis, the force of a rigid body can be expressed by a wrench. The driving forces/torques for the specified wrench on the end effector are expressed by $M={\left[\begin{array}{ccc}\begin{array}{cc}{M}_1\hfill & M_2\hfill \end{array}\hfill & \cdots \hfill & M_n\hfill \end{array}\right]}^T\in R^{n\times 1}$ and the wrench matrix of the external load acting on the end effector is expressed by $\$=\left[\begin{array}{c}F\hfill \\ \multicolumn{1}{c}{r_P\times F+\mathrm{pF}}\end{array}\right]\in R^{6\times 1}$ . The external load exerts on the end effector at point P. The wrench matrix is $. The position vector of the action point P in the absolute coordinate system is $r_P$ . When only a force acts, it is expressed by $\$=\left[\begin{array}{c}F\hfill \\ \multicolumn{1}{c}{r_P\times F}\end{array}\right]\in R^{6\times 1}$ .

The reciprocal screw theory indicates that the constraints exerted to the point attached to the end effector are reciprocal to its twist. Two screws, $\xi \in R^{6\times 1}$ and $\$\in R^{6\times 1}$ , are considered reciprocal when ${\$}^T\aleph \xi =0$ where $\aleph$ represents the reciprocal product of two screws. The physical meaning of ${\$}^T\aleph \xi$ is the instantaneous power of wrench $ to twist $\xi$ . ${\$}^T\aleph \xi =0$ is the condition for the spatial rigid body to keep balance.

Furthermore, the twist and wrench are expressed by using Plücker coordinates ²⁴

$$\xi ={\left[\begin{array}{cc}\begin{array}{ccc}{L}_1\hfill & M_1\hfill & N_1\hfill \end{array}\hfill & \begin{array}{ccc}{P}_1\hfill & Q_1\hfill & R_1\hfill \end{array}\hfill \end{array}\right]}^{\mathrm{T}},$$

(5)

$$\$={\left[\begin{array}{cc}\begin{array}{ccc}{L}_2\hfill & M_2\hfill & N_2\hfill \end{array}\hfill & \begin{array}{ccc}{P}_2\hfill & Q_2\hfill & R_2\hfill \end{array}\hfill \end{array}\right]}^{\mathrm{T}}.$$

(6)

In this way, the reciprocal product of two screws can be expressed as

$${\$}^T\aleph \xi =L_1{P}_2+M_1{Q}_2+N_1{R}_2+L_2{P}_1+M_2{Q}_1+N_2{R}_1$$

(7)

From the above discussion, we know that any generalized instant velocity of a rigid body can be described by a twist, and any set of generalized forces that act on a rigid body can be described as a wrench. A screw vector can be expressed by using the Plücker coordinates for a twist or wrench. ²⁴ By describing the motion and force using the twist and wrench, a unified description method can be established in screw form, which makes the mechanism analysis more easy.^22,23

Kinetostatics of the serial manipulator

Figure 1 shows the serial manipulator composed of multiple links connected by rotational pairs. The twist matrix is $S$ and the vectors of the joint velocities is $\omega ={\left[\begin{array}{cccc}{\omega }_1\hfill & {\omega }_2\hfill & \cdots \hfill & {\omega }_n\hfill \end{array}\right]}^T$ .

Figure 1.

A serial manipulator in space.

There is a torque input by the motor at each rotational joint in the kinematic chain. Assume that the instantaneous work done by the input torque is

$$d{W}_T=M^T\omega \mathrm{dt}.$$

(8)

According to the reciprocal screw theory, we gain that the instantaneous work done by the external load is

$$d{W}_L={\$}^T\aleph \xi \mathrm{dt},$$

(9)

where $\aleph =\left[\begin{array}{cc}0\hfill & I\hfill \\ \multicolumn{1}{c}{I}& 0\hfill \end{array}\right]$ and $I=\left[\begin{array}{ccc}1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill \end{array}\right]$ .

Because $\omega \mathrm{dt}={\left[\begin{array}{cccccc}d{\theta }_1\hfill & d{\theta }_2\hfill & \cdots \hfill & d{\theta }_i\hfill & \cdots \hfill & d{\theta }_n\hfill \end{array}\right]}^T$ , $d\theta =\omega \mathrm{dt}$ , equation (8) can be expressed as follows:

$$d{W}_T=M^{T}d\theta .$$

(10)

Equation (9) can be expressed as follows:

$$d{W}_L={\$}^T\aleph S\omega \mathrm{dt}={\$}^T\aleph \mathrm{Sd}\theta .$$

(11)

For a balanced kinematic system, equating the input and the output virtual powers, one obtains

$$d{W}_M+d{W}_L=0.$$

(12)

Substituting equations (10) and (11) into equation (12) presents

$$(M^T+{\$}^T\aleph S)d\theta =0.$$

(13)

Since $d\theta ={[d{\theta }_1\mathrm{d}{\theta }_2\cdots \mathrm{d}{\theta }_i\cdots \mathrm{d}{\theta }_n]}^T\ne 0$ , equation (13) can be simplified as

$$M^T=-{\$}^T\aleph S.$$

(14)

Therefore, torques for all joint motors are expressed in the vector form of $M$

$$M=-S^T\aleph \$.$$

(15)

Through the above analysis, we easily analyze the kinetostatics of serial manipulators. By programming the algorithm, the torques required by the motors of a serial robot can be easily obtained. It should be pointed out that the relationship between the angular displacement, time, and angular velocity during the moving process should be satisfied as follows:

$$\theta \left[\mathrm{\Delta }t(i+1)\right]=\theta \left(i\mathrm{\Delta }t\right)+\mathrm{\Delta }t\omega \left(i\right),$$

(16)

where $\theta \left(i\mathrm{\Delta }t\right)={\left[\begin{array}{cccc}{\theta }_1\left(i\mathrm{\Delta }t\right)\hfill & {\theta }_2\left(i\mathrm{\Delta }t\right)\hfill & \cdots \hfill & {\theta }_n\left(i\mathrm{\Delta }t\right)\hfill \end{array}\right]}^{\mathrm{T}}$ , $i=1,2,\cdots$ , and n is the number of joints. Consequently, the main steps to solve the kinetostatics of serial manipulators are as follows:

1. Determine the initial amount, including the length of each link $l_i$ , the size of the first joint angle $\theta \left(t_0\right)$ and the external load $.

2. Calculate other angle variables at $t=t_0$ according to the geometric relationship.

3. All angular displacements at $t=t_0$ is brought into equation (2), and the twist matrix $S\left(t_0\right)$ can be solved.

4. Substituting $ and $\mathit{S}$ into equation (15), the initial torques can be obtained.

5. To solve the input torques in a period, we should update the twist matrix $S(t_0+\mathrm{\Delta }t)$ through equation (16) to get the next moment.

6. Substituting the twist matrix $S(t_0+\mathrm{\Delta }t)$ at $t=t_0+\mathrm{\Delta }t$ into equation (15), and repeating steps 1–4, we get the motion curve of the motor input torque in a period.

In summary, the process of torque control and actuation distribution for the kinetostatics of a serial robot is shown in Figure 2. The process not only expresses the motion and operating force of the robotics system in a unified way but also realizes the real-time acquisition of joint torque and is convenient for control. Based on the kinetostatics strategy, the corrected joint torque for torque control and actuation distribution can be obtained.

Figure 2.

The process of the kinetostatic analysis of a serial robot.

Analysis of typical serial manipulators

A planar redundantly actuated robotic arm

Figure 3 illustrates a planar 4-DOF redundant robotic arm. Establish a coordinate frame shown in Figure 3, where the coordinates of the end effector are $(x,y)$ , and the angle between the i-th link and the horizontal direction is ${\theta }_i$ .

Figure 3.

The redundant robotic arm.

The twist matrix of the end effector can be gained from equation (1). At this time, $S=\left[\begin{array}{cccc}{\$}_1\hfill & {\$}_2\hfill & {\$}_3\hfill & {\$}_4\hfill \end{array}\right]$ and ${\$}_i={\left[\begin{array}{cccccc}0\hfill & 0\hfill & 1\hfill & y_i\hfill & -x_i\hfill & 0\hfill \end{array}\right]}^T$ , $(i=1\mathrm{to}4)$ .

In the absolute coordinate system, the coordinates of these joints are

$$r_{A_1}={\left[\begin{array}{ccc}0\hfill & 0\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},$$

(17)

$$r_{A_2}={\left[\begin{array}{ccc}{l}_1\cos {\theta }_1\hfill & l_1\sin {\theta }_1\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},$$

(18)

$$\begin{array}{c}\multicolumn{1}{c}{r_{A_3}={\left[\begin{array}{ccc}{l}_1\cos {\theta }_1+l_2\cos ({\theta }_1+{\theta }_2)\hfill & l_1\sin {\theta }_1+l_2\sin ({\theta }_1+{\theta }_2)\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},}\end{array}$$

(19)

$$\begin{array}{c}{r}_{A_4}=\hfill \\ {\left[\begin{array}{ccc}\begin{array}{c}{l}_1\cos {\theta }_1+l_2\cos ({\theta }_1+{\theta }_2)\hfill \\ \multicolumn{1}{c}{+l_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)}\end{array}\hfill & \begin{array}{c}{l}_1\sin {\theta }_1+l_2\sin ({\theta }_1+{\theta }_2)\hfill \\ \multicolumn{1}{c}{+l_4\sin ({\theta }_1+{\theta }_2+{\theta }_3)}\end{array}\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}}.\hfill \end{array}$$

(20)

Therefore, the screw matrix is

$$S=\left[\begin{array}{cccc}0\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{1}& 1\hfill & 1\hfill & 1\hfill \\ \multicolumn{1}{c}{0}& l_1\sin {\theta }_1\hfill & l_1\sin {\theta }_1+l_2\sin ({\theta }_1+{\theta }_2)\hfill & \begin{array}{c}{l}_1\sin {\theta }_1+l_2\sin ({\theta }_1+{\theta }_2)\hfill \\ \multicolumn{1}{c}{+l_4\sin ({\theta }_1+{\theta }_2+{\theta }_3)}\end{array}\hfill \\ \multicolumn{1}{c}{0}& -l_1\cos {\theta }_1\hfill & -l_1\cos {\theta }_1-l_2\cos ({\theta }_1+{\theta }_2)\hfill & \begin{array}{c}-l_1\cos {\theta }_1-l_2\cos ({\theta }_1+{\theta }_2)\hfill \\ \multicolumn{1}{c}{-l_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)}\end{array}\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill \end{array}\right].$$

(21)

The wrench of the end effector is

$${\$}^T={\left[\begin{array}{cccccc}0& P& 0& 0& 0& P{l}_1\cos {\theta }_1+P{l}_2\cos \left({\theta }_1+{\theta }_2\right)+P{l}_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\end{array}\right]}^T.$$

(22)

Based on the proposed kinetostatics analysis method, we get that

$$\begin{array}{c}\multicolumn{1}{c}{M=\left[\begin{array}{c}{M}_1\hfill \\ \multicolumn{1}{c}{M_2}\\ \multicolumn{1}{c}{M_3}\\ \multicolumn{1}{c}{M_4}\end{array}\right]=-S^T\aleph \$}\\ =\left[\begin{array}{c}P{l}_1\cos {\theta }_1+P{l}_2\cos ({\theta }_1+{\theta }_2)+P{l}_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)\hfill \\ \multicolumn{1}{c}{P{l}_2\cos ({\theta }_1+{\theta }_2)+P{l}_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)}\\ \multicolumn{1}{c}{P{l}_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)}\\ \multicolumn{1}{c}{0}\end{array}\right].\hfill \end{array}$$

(23)

The primary steps to get the input torque of each joint were given. The trajectory of the robot is also shown in Figure 4(a). The inverse kinematics can be referred to Zhao et al. ²² and Gan et al. ²⁵ and joint angular velocity was calculated, as shown in Figure 4(b).

Figure 4.

Motion parameters of a 4-DOF redundant robotic arm: (a) motion trajectory of the redundant robotic arm and (b) joint angular velocity of the redundant robotic arm.

Next, the kinostatics for redundantly robotic arm can be performed programmatically

1. Stipulate the length of each link and the angular displacement of each joint. Table 1 shows the initial parameters.

2. After all initial structure parameters at t = 0 are brought into equation (15), we get the initial torques at t = 0.

3. Through equation (16), the angular displacement will be replaced at the next moment. Finally, we obtain the motion performance of the robot under the input moment within a period.

Table 1.

Initial parameters of the redundant robotic arm.

$l_1\left(\mathrm{mm}\right)$	$l_2\left(\mathrm{mm}\right)$	$l_3\left(\mathrm{mm}\right)$	$l_4\left(\mathrm{mm}\right)$	${\theta }_1\left(0\right)\left(\mathrm{rad}\right)$	${\theta }_2\left(0\right)\left(\mathrm{rad}\right)$	${\theta }_3\left(0\right)\left(\mathrm{rad}\right)$	${\theta }_4\left(0\right)\left(\mathrm{rad}\right)$	$P\left(N\right)$
200	210	300	310	$\pi /3$	$\left(11\pi \right)/6$	$\left(4\pi \right)/3$	$\left(5\pi \right)/3$	20

The proposed algorithm is programmed, and through numerical calculation, the torque curve of each joint is illustrated in Figure 5. It can be seen that this algorithm solves the joint torques very succinctly.

Figure 5.

Torque of each joint for the 4-DOF redundant robotic arm: (a) torque of joint 1, (b) torque of joint 2, (c) torque of joint 3, and (d) torque of joint 4.

The torques of the motors at every joint of the serial robotic arm are changing cyclically as the robot starts to move from the initial parameters listed in Table 1.

4R spatial manipulator

Figure 6 shows a 4R spatial-type manipulator. The spatial type involves four twist screws, namely ${\$}_1$ , ${\$}_2$ , ${\$}_3$ , and ${\$}_4$ .

Figure 6.

A spatial-type 4R manipulator.

The position vector of all joints was calculated geometrically from Figure 6:

$$r_{O_1}={\left[\begin{array}{ccc}0\hfill & 0\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},$$

(24)

$$r_{O_2}={\left[\begin{array}{ccc}{l}_1\cos {\theta }_1\hfill & l_1\sin {\theta }_1\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},$$

(25)

$$r_{O_3}={\left[\begin{array}{ccc}(l_1+l_2\cos {\theta }_2)c_1\hfill & (l_1+l_2\cos {\theta }_2)\sin {\theta }_1\hfill & l_2\sin {\theta }_2\hfill \end{array}\right]}^{\mathrm{T}},$$

(26)

$$r_{O_4}={\left[\begin{array}{ccc}{a}_1\hfill & a_2\hfill & l_2\sin {\theta }_2+l_3\sin {\theta }_3\hfill \end{array}\right]}^{\mathrm{T}},$$

(27)

$$r_{O_5}={\left[\begin{array}{ccc}{a}_1\hfill & a_2\hfill & l_2\sin {\theta }_2+l_3\sin {\theta }_3+l_4\sin {\theta }_4\hfill \end{array}\right]}^{\mathrm{T}},$$

(28)

where $a_1=(l_1+l_2\cos {\theta }_2+l_3\cos {\theta }_3)\cos {\theta }_1$ , $a_2=(l_1+l_2\cos {\theta }_2+l_3\cos {\theta }_3)\sin {\theta }_1$ , $a_3=(l_1+l_2\cos {\theta }_2+l_3\cos {\theta }_3+l_4\cos {\theta }_4)\cos {\theta }_1$ , and $a_4=(l_1+l_2\cos {\theta }_2+l_3\cos {\theta }_3+l_4\cos {\theta }_4)\sin {\theta }_1$ .

These unit axial vectors for rotational joint are expressed as follows:

$$s_1={\left[\begin{array}{ccc}0\hfill & 0\hfill & 1\hfill \end{array}\right]}^{\mathrm{T}},$$

(29)

$$s_2={\left[\begin{array}{ccc}-\sin {\theta }_1\hfill & \cos {\theta }_1\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},$$

(30)

$$s_3={\left[\begin{array}{ccc}-\sin {\theta }_1\hfill & \cos {\theta }_1\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}},$$

(31)

$$s_4={\left[\begin{array}{ccc}-\sin {\theta }_1\hfill & \cos {\theta }_1\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}}.$$

(32)

These unit screws of pure rotation are expressed as follows:

$${\$}_i={\left[\begin{array}{cc}{s}_i\hfill & r_{O_i}\times s_i\hfill \end{array}\right]}^{\mathrm{T}}i=1\mathrm{to}4$$

(33)

As shown previously, the twist matrix of this spatial manipulator is

$$S=\left[\begin{array}{cccc}{\$}_1\hfill & {\$}_2\hfill & {\$}_3\hfill & {\$}_4\hfill \end{array}\right].$$

(34)

Furthermore, the end effector is utilized to realize the grasping operation of the object, and the external load is $F={\left[\begin{array}{ccc}{F}_x\hfill & F_y\hfill & F_z\hfill \end{array}\right]}^{\mathrm{T}}$ . The wrench of the external load is

$$\$=\left[\begin{array}{c}F\hfill \\ \multicolumn{1}{c}{r_{O_5}\times F}\end{array}\right].$$

(35)

Suppose $F_x=1.2N$ , $F_y=2N$ , $F_z=2N$ . The initial structural parameters are shown in Table 2. For the input to the motor and motion trajectory, please refer to Figure 7.

Table 2.

Structure parameters and initial values of the 4R spatial type manipulator.

$l_1\left(\mathrm{mm}\right)$	$l_2\left(\mathrm{mm}\right)$	$l_3\left(\mathrm{mm}\right)$	$l_4\left(\mathrm{mm}\right)$	${\theta }_1\left(0\right)\left(\mathrm{rad}\right)$	${\theta }_2\left(0\right)\left(\mathrm{rad}\right)$	${\theta }_3\left(0\right)\left(\mathrm{rad}\right)$	${\theta }_4\left(0\right)\left(\mathrm{rad}\right)$
80	280	280	100	$\pi /6$	$\left(2\pi \right)/3$	$\pi /6$	$\left(2\pi \right)/3$

Figure 7.

Motion parameters of the 4R spatial manipulator: (a) motion trajectory of the 4R manipulator and (b) joint angular velocity of the 4R manipulator.

Therefore, by substituting equations (34) and (35) into equation (15), the actuation of the 4R manipulator was obtained. The driving torque of the 4R manipulator can be calculated in a period (see Figure 8).

Figure 8.

The simulation results of the joint torque for the 4R spatial manipulator: (a) the torque of joint 1, (b) the torque of joint 2, (c) the torque of joint 3, and (d) the torque of joint 4.

This example shows that the kinetostatics algorithm proposed in this paper not only fits in with the planar serial mechanism but also suits the spatial mechanism of multiple degrees of freedom.

Six-axis spatial robot

There are 6-axis vectors of pure rotation in the 6-axis spatial robot shown in Figure 9. Each axis vector is attached to a joint. The 6-axis manipulator consists of two parts, namely, a primary joint and a minor joint. The six joints of the 6-axis robot are all rotary ones. There are six columns in the twist matrix that is expressed as follows:

$$S=\left[\begin{array}{cccccc}{\$}_1\hfill & {\$}_2\hfill & {\$}_3\hfill & {\$}_4\hfill & {\$}_5\hfill & {\$}_6\hfill \end{array}\right].$$

(36)

Figure 9.

The general case of a 6-axis robot.

The link coordinate system of the 6-axis robot is shown in Figure 10. An absolute coordinate system is established on the base, and an end coordinate system is attached to the end effector. $o_6{x}_6{y}_6{z}_6$ is the hand-grip coordinate system.

Figure 10.

Schematic diagram of a 6-axis robot.

The position vector of the point $O_6(p_x,p_y,p_z)$ is $r_{O_6}$ . Given a scenario where the end effector is processing a workpiece, it is required to analyze the output torque of each joint in every quasistatic equilibrium state. According to the working situation, the external force $F={\left[\begin{array}{ccc}{F}_x\hfill & F_y\hfill & F_z\hfill \end{array}\right]}^{\mathrm{T}}$ acting on the end effector can be expressed in a unified Plücker coordinate system, as shown in Figure 9. $F_x=1.2N$ , $F_y=2N$ , $F_z=2N$ , the size, direction, and position of $F_z$ remain unchanged. It is worth noting that the magnitudes of $F_x$ and $F_y$ do not change while the position and orientation change with different poses of the end effector. Ignoring the dynamic effects and frictional force, the external load is expressed in the form of a wrench for a quasistatic state $.

In this simulation, the angular velocity of the robot joint and the trajectory of the robot end effector are shown in Figure 11. The initial structural parameters are shown in Table 3.

Figure 11.

Motion parameters of the 6-axis robot: (a) motion trajectory of the 6-axis robot and (b) joint angular velocity of the 6-axis robot.

Table 3.

Geometric parameters and initial values of a 6-axis robot.

$l_1\left(\mathrm{mm}\right)$	$l_2\left(\mathrm{mm}\right)$	$l_3\left(\mathrm{mm}\right)$	$l_4\left(\mathrm{mm}\right)$	${\theta }_1\left(0\right)$ $\left(\mathrm{rad}\right)$	${\theta }_2\left(0\right)$ $\left(\mathrm{rad}\right)$	${\theta }_3\left(0\right)$ $\left(\mathrm{rad}\right)$	${\theta }_4\left(0\right)$ $\left(\mathrm{rad}\right)$	${\theta }_5\left(0\right)$ $\left(\mathrm{rad}\right)$	${\theta }_6\left(0\right)$ $\left(\mathrm{rad}\right)$
150	318	430	100	$\pi /2$	0	$-\pi /2$	0	0	0

In this case, the joint driving torques of the manipulator are used as the input. Based on the geometric parameters and initial values, the torques $M_i(i=1\mathrm{to}6)$ are calculated by using the kinetostatic algorithm mentioned above (see Figures 12 and 13).

Figure 12.

The torque of joint 1, 2, 3 for the 6-axis robot.

Figure 13.

The torque of joints 4, 5, and 6 for the 6-axis robot.

From Figures 12 and 13, we find that the motors at joints 1, 2, 3, 4, and 5 are changing cyclically. The torque of joint 6 is constantly zero. This is true from Figure 13 since all the external forces pass through the center of joint 6. Moreover, the maximum torque value of joint 5 is greater than that of joint 4. This is because $F_x>F_y$ under the same distance $l_4$ .

Based on the analysis of joint torque with a 6-axis robot, the numerical algorithms are efficient and comprehensible in calculating the torques of all joints. Furthermore, the joint torque of a spatial serial multi-chain robot can be dynamically controlled on time by applying this kinetostatics algorithm.

Discussion

The essence of kinetostatic analysis is to analyze the equivalent transformation of forces between different coordinate systems. The principle of equivalence is to make two force systems equivalent. The equivalence principle includes the static equilibrium principle and the virtual work principle. Next, we discuss the differences between the two classical methods and our methods.

Static transfer formula

According to Craig ¹ , it can be known that the static transfer formula between two neighbor links is

$${}^i{\mathit{f}}_i={}^i{\mathit{f}}_{i+1},$$

(37)

$${}^i{\mathit{n}}_i={}_{i+1}{{}^{i}R}^{i+1}{\mathit{n}}_{i+1}+{}^i{\mathit{r}}_{i+1}\times {}^i{\mathit{f}}_{i+1}$$

(38)

where $f_i$ is the force exerted by link $i-1$ on link $i$ , and $n_i$ is the torque exerted by link $i-1$ on the link $i$ , ${}_{i+1}^i\mathit{R}$ is the transformation matrix from coordinate system $i$ to coordinate system $i+1$ , and $r_{i+1}$ is the position vector shown in Figure 14.

Figure 14.

Static equilibrium of neighbor links.

The joint torque to keep the system in static equilibrium is the dot product of the joint axis vector and the torque vector applied on the link

$${\tau }_i{=}^i{n}_i^T{}^i{z}_i.$$

(39)

To solve the joint torque using this method, it is necessary to disassemble each joint and recurse the force transmission between two links one by one. Take the robot arm with $n$ rigid bodies connected in series as an example, each rigid body can provide three force equations and three torque equations. There are six unknowns at a joint, including three forces and three torques. When there are $n$ rigid bodies, a total of $6n$ equations need to be solved to calculate the driving torques. However, the number of driving torques is only $n$ . This method is effective, but the calculation is very large.

Force Jacobin matrix based on D-H convention

From the perspective of work, the force Jacobian matrix method presents the joint torques

$${\tau }_i=J^{T}F,$$

(40)

where $J$ is the Jacobian matrix of a serial robot and $F$ is the generalized force vector acting on the end effector in one Cartesian coordinate system.

The joint torque is the vector product of $J$ and $F$ . According to Craig ¹ and Angeles, ² the force Jacobian matrix is the transpose of the velocity Jacobian matrix. The existing algorithm to solve the velocity Jacobian matrix is the D-H method in a Cartesian coordinate system. This requires establishing a coordinate system for each joint and solving the position and orientation of the axis. The calculation formula is as follows:

$$J=[J_1,J_2,\cdots ,J_n].$$

(41)

For rotational joints,

$$J_i=\left[\begin{array}{c}{z}_{i-1}\times {}^{i-1}r\hfill \\ \multicolumn{1}{c}{z_{i-1}}\end{array}\right],$$

(42)

where $z_{i-1}$ is the unit vector of $i-1$ axis. ${}^{i-1}\mathit{r}$ is the position vector under the $i-1$ coordinate system. During calculation, it is necessary to transform each joint coordinate system into the primary coordinate system.

Using this method, it is necessary to write the transformation matrix of each joint coordinate system relative to the 0-coordinate system, which is a process of concatenation. When there are $n$ rigid bodies, $n$ coordinate systems need to be established. According to D-H convention, there are four parameters ( $a_i,{\theta }_i,{\alpha }_i,d_i$ ) and one transformation matrix ${}^{i-1}{\mathit{T}}_i$ between two adjacent coordinate systems. For example, the matrix operation required from coordinate system $n$ to coordinate system 0 is ${}^0{\mathit{T}}_n={}^0{\mathit{T}}_1{}^1{\mathit{T}}_2\cdots {}^{n-1}{\mathit{T}}_n$ . Each transformation matrix is ${}^{i-1}{\mathit{T}}_i=\left[\begin{array}{cccc}\cos {\theta }_i\hfill & -\cos {\alpha }_i\sin {\theta }_i\hfill & \cos {\alpha }_i\sin {\theta }_i\hfill & a_i\cos {\theta }_i\hfill \\ \multicolumn{1}{c}{\sin {\theta }_i}& \cos {\alpha }_i\cos {\theta }_i\hfill & -\sin {\alpha }_i\cos {\theta }_i\hfill & a_i\sin {\theta }_i\hfill \\ \multicolumn{1}{c}{0}& \sin {\alpha }_i\hfill & \cos {\alpha }_i\hfill & d_i\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]$ . In this process, each link needs to be iteratively calculated, and then the matrix inversion is solved. This method has greatly improved compared with the statics transfer method. However, a large number of matrix operations render the high computational cost of force Jacobin matrix method based on D-H convention.

Screw kinetostatics

The joint torque is the reciprocal product of the twist matrix and end effector force screw, as shown in equation (15).

The main advantage of the screw method is that the twist matrix is generated into the unit screw through each joint. Unlike the construction of the Jacobian matrix based on D-H convention, the screw kinetostatics can solve the twist matrix more easily because there are only two coordinate systems. On the one hand, if the two coordinate systems are established according to D-H convention, the unit screw only needs the third and fourth columns of the transformation matrix. On the other hand, the unit screw can be obtained more easily through the Rodrigues transformation. On the contrary, the D-H method usually involves complex calculations, and some scholars have compared them ²⁶ in the differential kinematics of robot manipulators.

Based on the discussions above, it can be concluded that our method is simpler than the statics transfer method in calculation steps and the force Jacobian method in calculation cost. In addition, our method has realized computer programming, and three cases verify the universality of the method.

Conclusions

A new approach is proposed for kinetostatics of serial manipulators. The method considers that the instantaneous work done by the external load is expressed with reciprocal screw. It makes good use of the fact that the screw of a rigid body is composed of force and moment of a marking point on the body that coincides with the origin to express the external loads of a linkage. The kinetostatic equation in screw form is very concise and can solve the torques of joint space. Compared with the static transfer formula, the method proposed in this paper can directly solve the joint torque of the robot without disassembling the joint. Compared with the force Jacobian matrix method, our method reduces the calculation in solving the twist matrix.

Representative redundant robotic arms and spatial manipulators were selected for case studies. The proposed method was used to analyze the manipulators, and the calculation results were validated through simulations. Through the kinetostatic analysis of typical serial mechanisms, this paper shows the process of establishing the torque model in Plücker coordinates. The results indicate that the proposed method is extremely straightforward and provides a very convenient way for programming at a computer. Hence, the method can be utilized in control and motion planning of a robot.