The Five-DOF Explosion-Removal Manipulator Based on Position and Velocity Feedforward Compensation Control

Abstract

The main problem with a robotic system arm is its sensitivity to time delays in the control process. Due to this problem, it is necessary to further optimize the control process of the system. One solution is to deal with the control accuracy and response speed issues of robotic arm joints, to improve the system’s response performance and enhance the system’s anti-interference ability. This paper proposes a speed feedforward and position control scheme for robotic arm joint control. The conclusion section shows that compared to traditional five-degree-of-freedom robotic arm systems, the addressed robotic arm control system has a lower tracking delay and better dynamic response performance. It can improve the system’s response performance while also enhancing its anti-interference ability.

1. Introduction

With the development and progress of social science and technology, robots are playing an increasingly important role in aerospace, medical equipment, explosion relief, and other fields [1,2]. An explosion-removal robot is a kind of robot that can take the place of people to investigate and deal with flammable and explosive dangerous goods in an extreme environment with an explosion risk. When the explosive disposal robot moves to the designated position, it carries out the operation task through the mechanical arm carried on the robot. The trajectory and holding position of the robot arm can be determined by visual observation within the visible range of human eyes. However, when the manipulator is in the over-the-line range, there will be a delay in remote image transmission, the robot arm joint control accuracy is not high, and the robot arm grasping task will have great interference [3]. Therefore, in the underlying control system of the robot arm, the accuracy and stability of joint control have always been the problems that need to be solved.

Gao et al. [4] designed a heavy-duty detonating robot that uses electric pushers as drive joints. The use of electric pushers is simple to control and allows for greater thrust. However, the robot arm is large in volume and the precision of the electric pusher control is not high. The robot arm produces large errors during the gripping process. The authors in [5] propose a nonlinear stability analysis of robot arm position control using a perturbation observer. The positional error can be reduced, and the dynamic response of the system can be improved, but the method needs to be further optimized in terms of robustness and noise sensitivity. Wang et al. [6] designed an adaptive slide robot control system with no sensory feedback. This strategy is effective for nonlinear precision control. However, the method requires a more accurate transmission model than adaptive slip mode control with sensory feedback in tracking performance. Wu et al. [7] proposed a reconfigurable robot force/position control method based on extended state observer constraints. Using Lyapunov theory to prove the stability of the system. This method can track and compensate for the target trajectory. However, the experiment only verified the position-tracking effect through simulation and did not conduct an actual test on the robotic arm device.

Based on the above research, this paper introduces a five-degree-of-freedom foldable manipulator. The manipulator is equipped with automated guided vehicles (AGVs) [8]. The AGVs control the chassis to move and work in complex terrain through remote commands. Then the explosive dangerous goods will be picked up and collected, and transported to the designated storage place. Due to the sensitivity of the five-degree-of-freedom robotic arm system to time-delay phenomena in the control process, further optimization of the system’s control process is needed. To address the control accuracy and response speed issues of robotic arm joints, to improve the system’s response performance while also enhancing the system’s anti-interference ability, this paper proposes a control scheme of the manipulator’s joints with velocity feedforward and position fusion [9]. A mathematical model of the robotic arm joint motor is established, and each joint motor adopts closed-loop feedback control of speed feedforward and position fusion. The joint motor loop uses a PID (Proportional Integral Derivative) controller, which can reduce the error and compensate for the uncertain disturbance. Finally, it is verified that the control accuracy of the robot arm joint can be guaranteed. At the same time, it can improve the rapid convergence of joint movement. The control system has a certain robustness to the variable load situation.

2. Five-Degree-of-Freedom Manipulator System

The physical prototype used in this paper is a 5-DOF foldable manipulator. The whole manipulator system is composed of two parts, namely the ground station system and the main body of the five-degree-of-freedom manipulator. The system structure is shown in Figure 1. The main body of the 5-DOF manipulator consists of eight parts, including a power management unit, image transmission module, high-definition camera, data transmission module, embedded controller, motor driver, manipulator joint, and ultrasonic limit unit.

Its working process is that the high-definition camera collects the real-time video at the end of the manipulator, and transmits the video signal to the ground station through the image transmission module. After that, the data transmission module is used to receive the remote-control command sent by the ground station. The drive command to the servo driver is sent through the STM32 controller data processing, and the manipulator’s joint performs the corresponding action. To make the mechanical arm run to the specified position more accurately, four groups of ultrasonic modules are added for limit operation based on using an encoder for positioning.

2.1. Structural Design of Five-Degree-of-Freedom Foldable Manipulator

The 5-DOF foldable manipulator consists of seven parts, namely the middle arm elbow, mechanical claw, wrist joint, elbow joint of the forearm, shoulder joint 1, shoulder joint 2, and mechanical body. The mechanical arm has a foldable function. When the mechanical arm is not working, the mechanical arm is in a folded state. When the mechanical arm is working, each joint is unfolded. The folding state structure of the manipulator is shown in Figure 2.

The limit range of the manipulator joint affects the workspace of the manipulator. According to the limit range of the joint, the trajectory of the manipulator can be better planned to make it reach the desired position reasonably. The range of motion of the manipulator joint is shown in

Table 1. Range of motion of manipulator joint.

Mechanical Claw	Wrist Joint	Elbow Joint of Forearm	Middle Arm Elbow	Shoulder Joint 2 (Retractable)	Shoulder Joint 1 (Rotation)
−360°~+360°	−360°~+360°	0~120°	0~180°	−30°~+210°	−180°~+180°

.

When the mechanical arm extends or retracts, it needs to accurately locate the position of the mechanical arm. Based on the control of the joint position of the mechanical arm, four external ultrasonic sensors are introduced to assist the positioning. The corresponding ultrasonic sensor information is collected through the stm32 controller and converted into the corresponding angle value of each joint [10]. Ultrasonic distance measurement adopts the echo detection method [11]. We take the forearm elbow joint as an example, as shown in Figure 3. The ultrasonic sensor has a sending part (TRIG) and a receiving part (ECHO). After the sending end sends the square wave to the forearm elbow joint (Forearm), the receiving end detects whether there is a signal return. x is the distance value of ultrasonic detection, y is the fixed position of ultrasonic on the middle arm, and y > 0, θ is the angle between the forearm and the middle arm, i.e., the required angle value. In the initial state of the manipulator, there is a relationship:

$$tan\theta =\frac{x}{y}$$

(1)

From the inverse trigonometric function, the elbow joint angle of the forearm is:

$$\theta =arctan\frac{x}{y}$$

(2)

We obtain the angle of the forearm elbow joint θ. After that, it is fused with the data fed back by the motor encoder to realize the redundant limit function.

2.2. Structural Design of Five-DOF Foldable Manipulator

The D-H parameter method is a method that Denavit and Hartenberg proposed in 1955 to describe the rules of the kinematic relationship of the mechanism using the parameters of the connecting rod. In this paper, the standard D-H method is used to model the manipulator. Figure 4 shows the standard D-H linkage coordinate system [12].

D-H model modeling steps:

Around $Z_i$ axis rotation angle ${\theta }_i$. This is rotation transformation, which is represented by the rotation transformation operator [13]:

$$Ro{t}_{z_{i-1}}\left({\theta }_i\right)=\left[\begin{array}{ccc}{\mathit{cos}\theta }_i& -{\mathit{sin}\theta }_i& 0\\ {\mathit{sin}\theta }_i& {\mathit{cos}\theta }_i& 0\\ 0& 0& 1\end{array}\right]$$

(3)

From $X_{i-1}$ axis moves $d_i$ offset to $X_i$ axis. This is the translation transformation, which is represented by the translation transformation operator:

$$Tran{s}_{z_{i-1}}\left(d_i\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]$$

(4)

From $Z_{i-1}$ Axis moves the link length of $a_i$ to $Z_i$ axis. Transform for translation:

$$Tran{s}_{z_{i-1}}\left(a_i\right)=\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(5)

Around $X_i$ axis rotates clockwise the connecting rod angle ${\alpha }_i$, transform for rotation:

$$Ro{t}_{z_{i-1}}\left({\alpha }_i\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathit{cos}\alpha }_i& -{\mathit{sin}\alpha }_i\\ 0& {\mathit{sin}\alpha }_i& {\mathit{cos}\alpha }_i\end{array}\right]$$

(6)

Then the general transformation matrix from {i−1} coordinate system to {i} coordinate system is:

$${}^{i-1}T_i=\left[\begin{array}{cccc}{\mathit{cos}\theta }_i& -{\mathit{sin}\theta }_i{\mathit{cos}\alpha }_i& {\mathit{sin}\theta }_i{\mathit{sin}\alpha }_i& a_i{\mathit{cos}\theta }_i\\ {\mathit{sin}\theta }_i& {\mathit{cos}\theta }_i{\mathit{cos}\alpha }_i& -{\mathit{cos}\theta }_i{\mathit{sin}\alpha }_i& a_i{\mathit{sin}\theta }_i\\ 0& {\mathit{sin}\alpha }_i& {\mathit{cos}\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right]$$

(7)

where ${}^{i-1}T$ represents the position and attitude of the i-th connecting rod relative to the i−1st connecting rod. The position and attitude of the end effector of the manipulator relative to the base coordinate system can be obtained by simply bringing in the D-H parameter of the connecting rod:

$${}^{0}T_i={}^{0}T_1{}^{1}T_2{}^{2}T_3\dots \dots {}^{i-1}T_i$$

(8)

The manipulator designed in this paper is a manipulator with five degrees of freedom of rotation. Due to the complex structure of the manipulator, to simplify the modeling difficulty, a virtual joint can be established between two real joints [14]. Due to being a virtual joint, its angle value is fixed. After the introduction of the virtual joint, the manipulator has a total of six joints, of which the virtual joint is located between the elbow joint and the wrist joint of the forearm. The structure diagram of the virtual joint and the coordinate system established according to the standard D-H method are shown in Figure 5, the virtual joint is marked in the figure.

Based on the coordinate system of each joint established in Figure 4, the corresponding D-H parameter table can be obtained, as shown in

Table 2. D-H parameter table of manipulator after introducing virtual joint.

Joint	${\mathit{d}}_{\mathit{i}}$	${\mathit{a}}_{\mathit{i}}$	${\mathit{\alpha }}_{\mathit{i}}$	${\mathit{\theta }}_{\mathit{i}}$	Joint Range/°
Shoulder joint 1	$d_1$	0	Pi/2	${\theta }_1$	0~180
Shoulder joint 2	0	$a_2$	0	${\theta }_2$	−60~180
Middle arm elbow	0	$a_3$	0	${\theta }_3$	−120~90
Virtual joint	0	$a_4$	0	${\theta }_4$	90
Elbow joint of forearm	0	$a_5$	Pi/2	${\theta }_5$	−90~90
Wrist joint	$d_6$	0	0	${\theta }_6$	−180~180

. Joint angle ${\theta }_i$ in the table: the angle from $X_{i-1}$ to $X_i$ around axis $Z_{i-1}$; Offset distance $d_i$: the distance from $X_{i-1}$ to $X_i$ along axis $Z_{i-1}$; Connecting rod length $a_i$: the distance from $Z_{i-1}$ to $Z_i$ along axis $X_i$; Connecting rod torsion angle ${\alpha }_i$: the angle of rotation from $Z_{i-1}$ to $Z_i$ around axis $X_i$ [15].

2.3. Positive Kinematics Analysis of Manipulator

The forward kinematics analysis is to calculate the position and posture of the robot’s end under the condition that the robot’s joint variables, namely the joint angles ${\theta }_i$ known. According to the coordinate transformation rule of the manipulator, the homogeneous coordinate transformation matrix of each joint can be derived from Table 2 and Formula (7).

To express concisely, $sin{\theta }_i$ and $cos{\theta }_i$ is recorded as $S_i$ and $C_i$. Add $sin({\theta }_m+{\theta }_n)$, $cos({\theta }_m+{\theta }_n)$ and $\left({\theta }_m+{\theta }_n\right)$ recorded as $S_{mn}$, $C_{mn}$ and ${\theta }_{mn}$, where i, m, n ∈ {0,1,2,3,4,5,6}.

$${}^{0}T_1=\left[\begin{array}{cccc}{C}_1& 0& S_1& 0\\ S_1& 0& -C_1& 0\\ 0& 1& 0& d_1\\ 0& 0& 0& 1\end{array}\right]$$

(9)

$${}^{1}T_2=\left[\begin{array}{cccc}{C}_2& -S_2& 0& a_2{C}_2\\ S_2& C_2& 0& a_2{S}_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(10)

$${}^{2}T_3=\left[\begin{array}{cccc}{C}_3& -S_3& 0& a_3{C}_3\\ S_3& C_3& 0& a_3{S}_3\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(11)

$${}^{3}T_4=\left[\begin{array}{cccc}{C}_4& -S_4& 0& a_4{C}_4\\ S_4& C_4& 0& a_4{S}_4\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(12)

$${}^{4}T_5=\left[\begin{array}{cccc}{C}_5& 0& S_5& a_5{C}_5\\ S_5& 0& {-C}_5& a_5{S}_5\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(13)

$${}^{5}T_6=\left[\begin{array}{cccc}{C}_6& -S_6& 0& 0\\ S_6& C_6& 0& 0\\ 0& 0& 1& d_6\\ 0& 0& 0& 1\end{array}\right]$$

(14)

We multiply the above formula to obtain the position and attitude of the center of the manipulator end actuator.

$${}^{0}T_6={}^{0}T_1{}^{1}T_2{}^{2}T_3{}^{3}T_4{}^{4}T_5{}^{5}T_6=\left[\begin{array}{cccc}{n}_x& o_x& q_x& p_x\\ n_y& o_y& q_y& p_y\\ n_z& o_z& q_z& p_z\\ 0& 0& 0& 1\end{array}\right]$$

(15)

In the formula:

$n_x=S_1{S}_6+C_{2345}{C}_1{C}_6$;

$n_y=C_{2345}{C}_6{S}_1-C_1{S}_6$;

$n_z=S_{2345}{C}_6$;

$o_x=C_6{S}_1-C_{2345}{C}_1{S}_6$;

$o_y=-C_1{C}_6-C_{2345}{S}_1{S}_6$;

$o_z=S_{2345}{S}_1$;

$q_x=S_{2345}{C}_1$;

$q_y=S_{2345}{S}_1$;

$q_z=-C_{2345}$;

$p_x=\left[a_5{C}_{2345}+d_6{S}_{2345}+a_3{C}_{23}+a_2{C}_2+a_4{C}_{234}\right]C_1$;

$p_y=\left[a_5{C}_{2345}+d_6{S}_{2345}+a_3{C}_{23}+a_2{C}_2+a_4{C}_{234}\right]S_1$;

$p_z=d_1-d_6{C}_{2345}+a_5{S}_{2345}+a_3{S}_{23}+a_2{S}_2+a_4{S}_{234}$;

The D-H parameters of the manipulator designed in this paper are known and at a given joint angle ${\theta }_i$, the pose matrix of the center point of the robot end actuator can be obtained through the above kinematics analysis.

2.4. Inverse Kinematics Analysis of Manipulator Based on Analytical Method

Inverse kinematics has a higher application value and is the basis of robot motion planning and trajectory planning [16]. There are usually two kinds of methods for solving inverse kinematics: the analytical method and numerical method. The analytical method is fast in operation, but has poor generality; the numerical method has high generality, but its solution speed is slow. By comparing the advantages and disadvantages of the two methods, the inverse kinematics analysis of the manipulator based on the analytical method is adopted in this paper. The sufficient condition for the manipulator to have the inverse kinematics analytical solution is to meet the Pieper criterion, i.e., to obtain the closed solution, one of two sufficient conditions needs to be satisfied [17,18,19].

(1)

The three adjacent joint axes intersect at a point;

(2)

The three adjacent joint axes are parallel to each other.

Shoulder joint 2, the middle arm elbow joint and the elbow joint of the forearm of the manipulator designed in this paper are parallel to each other, meeting the second sufficient condition, so the analytical solution can be obtained.

1.

We solve joint angle ${\theta }_1$

We multiply both sides of Formula (15) by the matrix ${{}^{0}T_1}^{-1}$, the following equation can be obtained:

$${}^{1}T_2{}^{2}T_3{}^{3}T_4{}^{4}T_5{}^{5}T_6={{}^{0}T_1}^{-1}\left[\begin{array}{cccc}{n}_x& o_x& q_x& p_x\\ n_y& o_y& q_y& p_y\\ n_z& o_z& q_z& p_z\\ 0& 0& 0& 1\end{array}\right]$$

(16)

The following matrix can be obtained by simplifying the right side of Equation (16):

$$\left[\begin{array}{cccc}{n}_x{C}_1+n_y{S}_1& o_x{C}_1+o_y{S}_1& q_x{C}_1+q_y{S}_1& p_x{C}_1+p_y{S}_1\\ n_z& o_z& q_z& p_z-d_1\\ n_x{S}_1-n_y{C}_1& o_x{S}_1-o_y{C}_1& q_x{S}_1-q_y{C}_1& p_x{S}_1-p_y{C}_1\\ 0& 0& 0& 1\end{array}\right]$$

(17)

The following matrix can be obtained by simplifying the left side of Equation (16):

$$\begin{array}{cccc}{C}_{2345}{C}_6& S_{2345}& C_{2345}{S}_6& a_5{C}_{2345}+d_6{S}_{2345}+a_3{C}_{23}+a_2{C}_2+a_4{C}_{234}\\ S_{2345}{C}_6& -C_{2345}& S_{2345}{S}_6& a_5{S}_{2345}+d_6{C}_{2345}+a_3{S}_{23}+a_2{S}_2+a_4{S}_{234}\\ S_6& C_6& 0& 0\\ 0& 0& 0& 1\end{array}$$

(18)

We observe the matrix (17) and (18), and select the element in the fourth column of the third row to obtain a ${\theta }_1$ and obtain the univariate linear Equation (19) [20],

$$p_x{S}_1-p_y{C}_1=0$$

(19)

It can be solved as follows:

$${\theta }_1=atan2\left(p_y,p_x\right)$$

(20)

2.

We solve joint angle ${\theta }_6$

We continue to observe the matrix (17) and (18), and select the elements in the first and second columns of the third row to obtain a system of binary linear equations about ${\theta }_6$ (21),

$$\left\{\begin{array}{c}{n}_x{S}_1-n_y{C}_1=S_6\\ o_x{S}_1-o_y{C}_1=C_6\end{array}\right.$$

(21)

Since ${\theta }_1$ is known, we can obtain:

$${\theta }_6=atan2\left(n_x{S}_1-n_y{C}_1,o_x{S}_1-o_y{C}_1\right)$$

(22)

3.

We solve joint angle ${\theta }_{2345}$

We continue to observe the matrix (17) and (18), and select the elements in the first and second rows of the second column to obtain a system of binary linear equations about ${\theta }_{2345}$ (23),

$$\left\{\begin{array}{c}{o}_x{C}_1+o_y{S}_1=S_{2345}\\ o_z=-C_{2345}\end{array}\right.$$

(23)

Since ${\theta }_1$ is known, we can obtain:

$${\theta }_{2345}=atan2\left(o_x{C}_1+o_y{S}_1,-o_z\right)$$

(24)

4.

We solve joint angle ${\theta }_4$

Since ${\theta }_4$ is a virtual joint, so the joint angle value is fixed as:

$${\theta }_4=pi/2$$

(25)

5.

We solve joint angle ${\theta }_3$

We continue to observe the matrix (17) and (18), and select the elements in the first and second rows of the fourth column to obtain a system of binary linear equations about ${\theta }_{23}$ and ${\theta }_{234}$ (26)

$$\left\{\begin{array}{c}{a}_5{C}_{2345}+d_6{S}_{2345}+a_3{C}_{23}+a_2{C}_2+a_4{C}_{234}=p_x{C}_1+p_y{S}_1\\ a_5{S}_{2345}+d_6{C}_{2345}+a_3{S}_{23}+a_2{S}_2+a_4{S}_{234}=p_z-d_1\end{array}\right.$$

(26)

We move the term of equation group (26), move the known term to the left, and the unknown term to the right, and ${\theta }_4$ is known, so the trigonometric function identity transformation is used, and the equation set (27) can be obtained by simplifying the term of ${\theta }_4$:

$$\left\{\begin{array}{c}{p}_x{C}_1+p_y{S}_1-a_5{C}_{2345}-d_6{S}_{2345}=a_3{C}_{23}+a_2{C}_2-a_4{S}_{23}\\ p_z-d_1-a_5{S}_{2345}+d_6{C}_{2345}=a_3{S}_{23}+a_2{S}_2+a_4{C}_{23}\end{array}\right.$$

(27)

We add the two sides of the equation set (4.25) after the square, which can be eliminated using the elimination method ${\theta }_2$. At the same time, using the triangular transformation formula, we can obtain:

$$A^2+B^2-a_2^2-a_3^2-a_4^2=2a_2{a}_3{C}_3-2a_2{a}_4{S}_3$$

(28)

In the formula:

$$\begin{array}{c}A=p_x{C}_1+p_y{S}_1-a_5{C}_{2345}-d_6{S}_{2345};\\ B=p_z-d_1-a_5{S}_{2345}+d_6{C}_{2345}\end{array}$$

Using the trigonometric identity transformation, it is assumed that:

$$\left\{\begin{array}{c}\eta \mathit{sin}\phi =a_3\\ \eta \mathit{cos}\phi =a_4\end{array}\right.$$

(29)

Then:

$$\eta =\sqrt{a_3^2+a_4^2},\phi =atan2\left(a_3,a_4\right)$$

(30)

By introducing Equation (29) into Equation (28), we can obtain:

$$\left\{\begin{array}{c}\mathit{sin}\left(\phi -{\theta }_3\right)=\frac{A^2+B^2-a_2^2-a_3^2-a_4^2}{2a_2\eta }\\ \mathit{cos}\left(\phi -{\theta }_3\right)=\pm \sqrt{1-{\left(\frac{A^2+B^2-a_2^2-a_3^2-a_4^2}{2a_2\eta }\right)}^2}\end{array}\right.$$

(31)

According to Equation (31):

$$\begin{array}{l}{\theta }_3=atan2\left(a_3,a_4\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}-atan2(A^2+B^2-a_2^2-a_3^2\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}-a_4^2,\pm \sqrt{\left[4a_2^2\left(a_3^2+a_4^2\right)-\left(A^2+B^2-a_2^2-a_3^2-a_4^2\right)\right]})\end{array}$$

(32)

6.

We solve joint angle ${\theta }_5$

So far, the values of ${\theta }_1$, ${\theta }_3$, ${\theta }_4$, ${\theta }_6$, ${\theta }_{2345}$ have been solved. We continue to observe the matrices (17) and (18), and the solution to obtain ${\theta }_5$ continues to simplify Equation (16) and multiply both sides of Equation (16) by ${{}^{5}T_6}^{-1}$, ${{}^{4}T_5}^{-1}$, and the matrix can be obtained by simplifying the left side of the equation:

$$\left[\begin{array}{cccc}{C}_{234}& -S_{234}& 0& a_3{C}_{23}+a_2{C}_2+a_4{C}_{234}\\ S_{234}& C_{234}& 0& a_3{S}_{23}+a_2{S}_2+a_4{S}_{234}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(33)

On the right side of the simplified Equation (16), we can obtain ${\theta }_5$. Because the matrix is too complex, only the second row of elements needed is listed here:

$$\left[\begin{array}{cccc}{q}_z{S}_5+n_z{C}_5{C}_6-o_z{C}_5{S}_6& n_z{C}_6{S}_5+q_z{C}_5-o_z{S}_5{S}_6& o_z{C}_6+n_z{S}_6& p_z-d_1-q_z{d}_6-a_5{n}_z{C}_6+a_5{o}_z{S}_6\end{array}\right]$$

(34)

By observing the matrix (33) and (34), select the elements in the first and second columns of the second row to obtain the information about ${\theta }_5$.

$$\left\{\begin{array}{c}{q}_z{S}_5+n_z{C}_5{C}_6-o_z{C}_5{S}_6=S_{234}\\ n_z{C}_6{S}_5+q_z{C}_5-o_z{S}_5{S}_6=C_{234}\end{array}\right.$$

(35)

At this point, the trigonometric function equation of ${\theta }_{234}$ and ${\theta }_{2345}$ are brought into the equation group (26), and the equation transformation is performed again to obtain the following formal equation group:

$$\left\{\begin{array}{c}{a}_2{C}_2=P-\left(X{S}_5+Y{C}_5\right)\\ a_2{S}_2=Q-\left(X{C}_5+Y{S}_5\right)\end{array}\right.$$

(36)

In the formula:

$$\begin{array}{c}N=n_z{C}_6-o_z{S}_6;\\ P=-(-a_5{q}_z+d_6\left(q_x{C}_1+q_y{S}_1\right)-p_x{C}_1-p_y{S}_1);\\ Q=-\left(a_5\left(q_x{C}_1+q_y{S}_1\right)+d_6{q}_z-p_z+d_1\right);\\ X=a_3{q}_z+a_{4}N;\\ Y=a_{3}N-a_4{q}_z\end{array}$$

We use the elimination method to square and add the two sides of the equation group (36), which can be eliminated ${\theta }_2$ relevant elements, the following equation is obtained:

$$2\left(QY-PX\right)S_5-2\left(QX+PY\right)C_5=a_2^2-P^2-Q^2-X^2-Y^2$$

(37)

Using the trigonometric identity transformation, it is assumed that:

$$\left\{\begin{array}{c}\eta \mathit{sin}\phi =2\left(QY-PX\right)\\ \eta \mathit{cos}\phi =2\left(QX+PY\right)\end{array}\right.$$

(38)

Then:

$$\eta =\sqrt{2{\left(QY-PX\right)}_{}^2+2{\left(QX+PY\right)}_{}^2}$$

(39)

$$\phi =atan2\left(2\left(QY-PX\right),2\left(QX+PY\right)\right)$$

(40)

By introducing Equation (38) into Equation (37), we can obtain:

$$\begin{array}{l}{\theta }_5=atan2\left(\pm \sqrt{\left(M-{\left(P^2+Q^2+X^2+Y^2-a_2^2\right)}^2\right)},P^2+Q^2+X^2+Y^2-a_2^2\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}-atan2\left(2\left(QY-PX\right),2\left(PY+QX\right)\right)\end{array}$$

(41)

In the formula:

$$\begin{array}{c}N=n_z{C}_6-o_z{S}_6;\\ P=-(-a_5{q}_z+d_6\left(q_x{C}_1+q_y{S}_1\right)-p_x{C}_1-p_y{S}_1);\\ Q=-\left(a_5\left(q_x{C}_1+q_y{S}_1\right)+d_6{q}_z-p_z+d_1\right);\\ X=a_3{q}_z+a_{4}N;\\ Y=a_{3}N-a_4{q}_z;\\ M=4\left({\left(QY-PX\right)}^2+{\left(PY+QX\right)}^2\right)\end{array}$$

7.

We solve joint angle ${\theta }_2$

According to the principle that joint angles can be vector added, we can obtain the joint angle of ${\theta }_2$:

$${\theta }_2={\theta }_{2345}-{\theta }_3-{\theta }_4-{\theta }_5$$

(42)

We can obtain four joint combinations of the manipulator in the same position and posture based on Formulas (32) and (41). Combined with the rotatable angle range of each joint of the manipulator, when the target position and pose T of the end effector of the manipulator is given, the inverse kinematics solution is performed, and only one of the four joint combinations obtained meets the rotation value range of the joint.

3. Kinematics Simulation Verification of Manipulator

3.1. Simulation and Verification of Forward Kinematics Equation

The accuracy of the analysis must be verified by forward kinematics analysis after the forward kinematics equation of the 5-DOF foldable anti-explosion manipulator is obtained. The pose matrix of the end effector of the manipulator is obtained by substituting the given joint angles of the manipulator into the forward kinematics equation. The pose matrix is compared with the pose matrix obtained using the MATLAB Robotic Toolbox. If they are equal, the positive kinematics equation is correct. The verification process of forward kinematics analysis is shown in Figure 6.

The D-H parameter of the 5-DOF foldable manipulator obtained from the parameter table: the length of the connecting rod $a_2=0.5\mathrm{m}$, $a_3=0.125\mathrm{m},a_4=0.366\mathrm{m},a_5=0.075\mathrm{m}$; Joint offset $d_1=0.2\mathrm{m},d_6=0.3\mathrm{m}$. At the same time, we change the initial joint angle ${\theta }_i$. (i = 1,2,3,5,6) is assigned to 0, i.e., ${\theta }_i=0°$, (i = 1,2,3,5,6), the virtual joint angle value is fixed, ${\theta }_4=90°$. We bring the above data into Equation (2) to obtain the position and attitude matrix of the manipulator end effector relative to the base coordinate system:

$${}^{0}T_6=\left[\begin{array}{cccc}0& 0& 1& 0.925\\ 0& -1& 0& 0\\ 1& 0& 0& 0.641\\ 0& 0& 0& 1\end{array}\right]$$

(43)

Using the MATLAB Robotic Toolbox, it is convenient to obtain the position and attitude matrix of the manipulator end effector relative to the base coordinate system. Given the joint space matrix of manipulator $\theta =\left[\begin{array}{cccccc}0& 0& 0& P_i/2& 0& 0\end{array}\right]$, we use the fkine forward kinematics solution function contained in the MATLAB Robotic Toolbox to obtain the position and posture matrix of the manipulator end effector, and the calculation results are shown in Figure 7.

Through comparison, the position and posture matrix of the end effector of the manipulator solved by the MATLAB Robotic Toolbox is completely consistent with the result obtained by Equation (3), which shows that the forward kinematics analysis result of the upper arm is correct. At the same time, the matrix of the manipulator in the manipulator joint space can be obtained when θ = [0,0,0, $P_i/2$,0,0] using the MATLAB Robotic Toolbox, as shown in Figure 8, where the coordinate units of the X, Y, and Z axes in this spatial matrix are all m.

3.2. Simulation and Verification of Inverse Kinematics Equation

Through the inverse kinematics analysis, the accuracy of the analysis must be verified after the inverse kinematics equation of the 5-DOF foldable anti-explosion manipulator is obtained. Through the given pose matrix of the manipulator end effector, the angle of each joint of the manipulator is solved by substituting it into the inverse kinematics equation, and the angle is compared with the angle value obtained using the MATLAB Robot Toolbox. If they are equal, the inverse kinematics equation is correct. The validation process of inverse kinematics analysis is shown in Figure 9.

To prove the correctness of the inverse kinematics algorithm of the manipulator, the joint space matrix of the manipulator is given first θ = [$P_i/3$, $P_i/3$, −$P_i/3$, $P_i/2$, $P_i/6$, $P_i/2$]. The forward kinematics solution of the manipulator, namely the position and posture matrix of the manipulator end actuator is obtained through the forward kinematics algorithm of the manipulator, and it is substituted into the inverse kinematics equation to obtain four inverse kinematics solutions, as shown in

Table 3. Four groups of inverse kinematics solutions.

Order Number	Joint	$\mathit{\theta }/\mathit{r}\mathit{a}\mathit{d}$	Joint	$\mathit{\theta }/\mathit{r}\mathit{a}\mathit{d}$
1	Shoulder joint 1	1.0472	Shoulder joint 2	0.9839
	Middle arm elbow	−1.0472	Virtual joint	1.5708
	Elbow joint of forearm	0.5869	Wrist joint	1.5708
2	Shoulder joint 1	1.0472	Shoulder joint 2	1.0472
	Middle arm elbow	−1.0472	Virtual joint	1.5708
	Elbow joint of forearm	0.5236	Wrist joint	1.5708
3	Shoulder joint 1	1.0472	Shoulder joint 2	1.1038
	Middle arm elbow	−1.1671	Virtual joint	1.5708
	Elbow joint of forearm	0.5869	Wrist joint	1.5708
4	Shoulder joint 1	1.0472	Shoulder joint 2	1.1671
	Middle arm elbow	−1.1671	Virtual joint	1.5708
	Elbow joint of forearm	0.5236	Wrist joint	1.5708

. The second group of solutions and space matrix θ = [$P_i/3$, $P_i/3$, −$P_i/3$, $P_i/2$, $P_i/6$, $P_i/2$] are the same, which verifies the correctness of the inverse kinematics algorithm.

The forward and inverse kinematics analysis and simulation verification of the manipulator can provide the theoretical and data basis for the actual motion control of the manipulator. The spatial analysis simulation, trajectory planning, and simulation of the manipulator joint all need the theoretical support of forward and inverse kinematics analysis.

4. Ground Station Control System

The ground station control system consists of three parts, as shown in Figure 10, which are the power management system, video unit, and manipulator controller. The power management system provides the ground station system power supply voltage. The video unit receives the video signal through the image transmission module, uses the display to return the real-time picture, and saves the video to the mobile hard disk through the recorder. The manipulator controller interface is composed of a joint key, power switch, power indicator, data transmission antenna, image transmission antenna, and image display screen. The robot arm can be remotely controlled through the robot arm control box to carry out operation tasks, and the end image of the robot arm can be displayed in real time through the display screen.

5. Mechanical Arm Joint Composition and Mathematical Model Establishment

5.1. Parameters of Wrist Joint Motor

Traditional DC motors use brushes for mechanical commutation [21], so there is mechanical friction, which shortens the service life of the motor and presents problems such as noise, sparks, and radio interference. In addition, high manufacturing costs and difficult maintenance limit its application on some special occasions. A brushless DC motor has high efficiency, long life, low noise, and good speed-torque characteristics [22]. Considering the above factors, this paper selects a brushless DC motor as the joint drive motor of the manipulator. Because the five joint motors in the manipulator all use the same control method, the fifth-axis wrist joint is selected for analysis in this paper. The wrist joint motor parameters are shown in Table 2.

Figure 11 shows the wiring diagram of the power supply, driver, and DC brushless motor. The input voltage of the switching power supply is AC 220 V, and the output voltage is DC 48 V, providing the power supply voltage of the driver and servo motor. The controller communicates with the driver using RS232 protocol, and the motor is connected to the driver through a network cable. The CAN bus protocol is used for communication between the controller and driver [23]. The modular design requires only one network cable to connect, which simplifies the wiring steps and saves the working space. The overall control process of the robotic arm is shown in Figure 12. The relevant parameters and interface functions of the controller are shown in

Table 4. The relevant parameters and interface functions of the controller.

Basic Parameters	Numerical or Interface Functions
MCU	STM32F103
Communication protocol	CAN bus protocol
Baud rate	500 K Bit/s
Sampling time point	1.75 μs
CAN interface	Used for communication of functional testing
JTAG interface	Used for DAP online simulation and program download

.

5.2. Establishment of Mathematical Model of Wrist Joint Motor (Constraint Conditions of Manipulator)

The mathematical equation model of the wrist joint motor is established. The voltage of any phase in the winding can be expressed as [24]:

$$u_x=R_x{i}_x+e_{\psi x}$$

(44)

where $u_x$ refers to phase voltage, subscript x refers to three-phase winding A, B, C, $i_x$ refers to phase current, $e_{\psi x}$ is phase-induced potential, $R_x$ is phase resistance, $R_a=R_b=R_c=R$ in three-phase symmetrical winding.

$$e_{\psi x}=\frac{d\psi x}{dt}$$

(45)

Because each phase winding and its current produces flux cross-chain, it also produces flux and permanent magnet cross-chain with other winding currents. Take phase A winding as an example:

$${\psi }_A=L_A{i}_A+M_{AB}{i}_B+M_{AC}{i}_C+{\psi }_{pm}\left(\theta \right)$$

(46)

In the formula, ${\psi }_{pm}\left(\theta \right)$ is the permanent magnetic linkage of the turning chain of phase A winding, $\theta$ is the rotor position angle, $L_A$ is self-induction of phase A winding; $M_{AB}$ and $M_{AC}$ are the mutual inductance of phase B winding and phase C winding to phase A winding.

Phase A voltage can be derived from Formula (44) to Formula (46) [25]:

$$u_A={Ri}_A+\left(L-M\right)\frac{d{i}_A}{dt}+e_A$$

(47)

After deduction, similar results are obtained for phases B and C. Since phase voltage is not easy to measure in practical applications, the mathematical model of line voltage is more in line with the actual situation, expressed as [26]:

$$\left[\begin{array}{c}{u}_{AB}\\ u_{BC}\\ u_{CA}\end{array}\right]=\left[\begin{array}{ccc}R& -R& 0\\ 0& R& -R\\ -R& 0& R\end{array}\right]\left[\begin{array}{c}{i}_A\\ i_B\\ i_C\end{array}\right]+\left[\begin{array}{ccc}L-M& M-L& 0\\ 0& L-M& M-L\\ M-L& 0& L-M\end{array}\right]\frac{d}{dt}\left[\begin{array}{c}{i}_A\\ i_B\\ i_C\end{array}\right]+\left[\begin{array}{c}{e}_A\\ e_B\\ e_C\end{array}\right]$$

(48)

Electromagnetic torque:

$$T_e=\frac{e_A{i}_A+e_B{i}_B+e_C{i}_C}{\mathsf{\Omega }}$$

(49)

Motor motion equation:

$$T_e-T_L=J\frac{d\mathsf{\omega }}{dt}+B_v\omega$$

(50)

where $T_L$ is the load torque, $J$ is the rotor rotational inertia, and $B_v$ is the viscous friction coefficient.

To control the brushless DC motor, it is necessary to establish the state equation of the brushless DC motor, which can be obtained by algebraic transformation of the differential mathematical model.

5.3. Design of Wrist Joint Based on Position PID Control

The joint motor of the manipulator adopts position closed-loop control to form a feedback loop in the control chain to reduce the accumulated error of the manipulator during the movement process and improve the accuracy of the manipulator joint tracking. The encoder feeds back the joint angle information to the controller, feeds back the actual current of the current motor through the current sensor, and adds speed feedforward based on the position control. On the premise of ensuring the accuracy of the joint control of the manipulator, it can improve the dynamic response ability of the joint and make the joint of the manipulator more flexible and smoother in the process of rotation.

The robot arm joint motor uses position closing control to form a feedback loop in the control chain. The classical PID controller [27] can reduce the additional error caused by the robot arm in the process of motion, and improve the accuracy of robot arm joint tracking. Among them, the digital increment PID controller can improve the reliability of the system, and the calculation amount of the algorithm is small. The algorithm is used as the joint control strategy of the manipulator, as shown in Formula (51)

$$u(k)=\hspace{0.33em}{K}_P\left[e(k)+\frac{T}{T_I}{\sum }_{j=0}^{k}e\left(j\right)+\frac{TD}{T}(e(k)-e(k-1))\right]$$

(51)

$$\begin{array}{l}△u\left(k\right)\hspace{0.33em}\hspace{0.33em}=\hspace{0.33em}\hspace{0.33em}u\left(k\right)-u\left(k-1\right)\\ \hspace{1em}\hspace{1em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}=\hspace{0.33em}\hspace{0.33em}{K}_P\left(e\right(k)-e(k-1\left)\right)+K_{I}e\left(k\right)+K_D\left(e\right(k)-\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}2e(k-1)+e(k-2))\end{array}$$

(52)

In Equations (51) and (52), $K_P$ is the proportional factor, $K_I$ represents the integration factor; $K_D$ is the division factor. $T$ represents the sampling period; $e\left(k\right)$, $e(k-1)$ represents the input error of $k$, $k-1$ sampling time. During the experiment, the changes in the three parameters $K_P$, $K_I$, and $K_D$ of the PID will affect the deviation between the actual value and the expected value, as well as the time when the actual value approaches the expected value. $K_P$ is used to accelerate system response speed and improve system adjustment accuracy. When there is a deviation between the actual value and the expected value, the regulator immediately amplifies the deviation output; $K_I$ is used to eliminate steady-state errors. If there is a deviation between the actual value and the expected value, even if the deviation may be small, the longer it exists, the larger the output signal. When the deviation is eliminated, the output stops changing; $K_D$ is used to improve the steady-state performance of the system. The larger the $K_D$ adjust, the better the system can respond in advance, but it will also amplify unnecessary deviations [28,29]. The PID control parameter tuning method tuning steps are first proportional, then integral, then differential. First, adjust the proportional parameters, reduce the proportional value by 10–20%, gradually add the integration time from large to small, until a 4:1 attenuation process is obtained. After adjusting the proportion and integration parameters, increase the proportion value by 10–20%, and then gradually add the differential time from small to large. Watch the transition curve until a satisfactory transition process is obtained.

The encoder feeds back the joint angle and angular velocity information to the position loop and the speed loop, respectively. The current loop uses the current sensor to feedback on the actual current of the current motor. In the DC brushless motor control, the most commonly used is position control [30], in which the manipulator tracks the required position through control commands. This method is very accurate in a stable state and can ensure the accuracy of repeated positioning, but the dynamic performance of individual position control is poor [31]. Feedforward control [32] can compensate for disturbance and has good momentum performance. Therefore, this paper adds speed feedforward control based on position control. The general block diagram of wrist joint motor control is shown in Figure 13.

(1)

Current loop PI control

The current loop adopts a PI controller [33], namely proportional and integral regulation, to limit the maximum current of the motor, regulate the dynamic structure of the object, and speed up the dynamic response of the system. The block diagram of the current loop is shown in Figure 14.

(2)

Speed loop PI control

The function of the speed loop is that the actual speed changes with the expected value to achieve no static error. The system has a certain anti-interference ability through closed-loop control, as shown in Figure 15.

(3)

Position loop control P and Feedforward control C

After the speed loop and current loop are adjusted, the desired speed can be obtained by differentiating the given positioning device. The position loop can track the given position using a proportional (P) controller, but the proportional control will have tracking lag. Based on position control, speed feedforward control is added to improve the response of the joint control system and reduce the position and speed errors. As shown in Figure 16. The simulation parameters of the system are shown in

Table 5. Simulation parameters.

Basic Parameters	Numerical Value
Proportional gain $K_p$	200
Integral term $K_I$	3.3
Differential term $K_D$	2.2
Simulation time T	0.3 s

.

5.4. Simulation of the Established Dynamic Model

According to the wrist joint position control block diagram and the differential equation of the DC brushless motor, the Simulink mathematical simulation model is established. This paper uses the Simscape module of Simulink, which is very intuitive for modeling and simulation analysis of the wrist joint motion system. To verify the correctness of the designed controller parameters, the simulation conditions are set as follows: the initial position is 0 degrees, the expected position is 10 degrees, the initial load torque $T_L$ is 0 N·m, and the load torque is 5 N·m, when it is equal to 0.2 s.

As shown in Figure 17, the motor will generate a large current when starting, and the system will reach a stable state in about 0.08 s. In 0.2 s, when 5 N·m load torque is applied, the current loop quickly reaches the expected current and remains stable, which verifies that the current loop control meets the control requirements of the manipulator joint.

According to the simulation results of the speed in Figure 18, the speed of the motor experienced two processes. The speed of the motor first increases from zero, and then decelerates to about 0.08 s, so it can quickly reach a stable state. The system has a fast dynamic response capability. After adding 5 N load in 0.2 s, the speed fluctuates slightly and returns to a stable state in a short time, which indicates that the control system is robust to variable load.

From the position simulation diagram in Figure 19, it can be seen that when the wrist joint motor moves from the initial position of 0 to 10 degrees, the slope of the position curve first increases and then decreases, and reaches the required position in about 0.08 s. After adding a 5 N load at 0.2 s, the position loop has a small tracking error, which can meet the needs of actual motor control performance.

According to the above simulation results, it can be verified that the feedforward and position control strategies have good dynamic performance and anti-interference ability, and can meet the accuracy requirements of wrist joint motor control.

6. Wrist Joint Motor Test

Actual Test Data of Position, Speed, and Current under Different Loads

The actual test was performed on the forearm elbow joint. This test is divided into two groups. The first group of wrist joints is not loaded. The second group of mechanical arm wrist joints increased the load by 0.5 kg, as shown in Figure 20.

The first group is the no-load test.

Figure 21a shows the expected current and actual current curve of the wrist joint motor. The green curve represents the expected current and the white curve represents the actual current. It can be seen from the figure that the actual current can well track the expected current through feedforward control. As shown in Figure 21b, the green curve is the expected speed curve, and the white curve is the actual speed curve. Under position control, the motor accelerates first, then decelerates, reaches the maximum speed of 250 rpm in about 0.15 s, and finally remains unchanged. As shown in Figure 21c, the wrist joint motor moves from the initial position of 0 to 1000 counts. The green curve is the expected position of the wrist joint, and the white curve is the actual position of the wrist joint. It is slightly too large at the actual position of about 0.18 s. Then, we track the desired position on the actual position, and the system reaches a stable state.

From the above actual test results, it can be seen that under the feedforward control, the actual position of the wrist joint can quickly track the desired position.

The second group is the wrist joint position, speed, and current curve after adding 0.5 kg load.

As shown in Figure 22a, the expected current curve and the actual current curve after the motor is loaded. It can be seen from the figure that under the feedforward control, the actual current curve can better track the expected current curve without delay. As shown in Figure 22b, the curve between the expected speed and the actual speed of the wrist joint motor and the load is shown. When rotating at a certain angle, the actual speed of the wrist motor can quickly track the desired speed, but there will be some errors. As shown in Figure 22c, after the wrist joint bears a load of 0.5 kg, it moves back and forth. The green curve is the expected position of the wrist joint, and the white curve is the actual position of the wrist joint. In about 0.18 s, the actual position shows obvious supersimulation. After the controller is adjusted, the desired position is tracked on the actual position within 0.3 s, and the system reaches a stable state.

The wrist joint-related parameters under no-load and load conditions are organized as shown in

Table 6. Experimental results of current, speed, and position under no-load and loaded conditions.

Wrist Joint	Motor Current	Motor Speed	Wrist Joint Position
No load	During the period of 0.12 s to 0.19 s, the expected current rises from 0.07 A to 0.62 A and then changes to −0.58 A, then fluctuates within the range of −0.1 to 0.1 A. After detecting significant fluctuations in the expected current within 0.12 s, corresponding adjustments were made, and the overall trend of change is the same as the expected current	During the period from 0.12 s to 01.19 s, the expected speed of the wrist joint motor increased from 0 to 258 rpm, then decreased to −25 rpm, and stabilized around 0 in 0.2 s. During the period from 0.12 s to 01.19 s, the actual speed increased from 0 to 238 rpm and then decreased to −17 rpm, with a delay of about 0.01 s compared to the expected speed curve.	At 0.12 s, the expected position of wrist joint movement suddenly changed from 0 to 1000, and the actual movement position continued to rise between 0.12 s and 0.18 s, stabilizing around 1000 around 0.19 s.
With load	During the period from 0.12 s to 0.19 s, there was a significant change in the expected current, rising from 0.43 A to 3.03 A and then dropping to −0.56 A. After 0.33 s, there was a slight fluctuation around 0.76 A; During the period from 0.12 s to 0.19 s, the actual current increased from 0.42 A to 2.95 A, then decreased to −0.54 A, and fluctuated slightly around 0.75 A after 0.32 s. The actual current curve can closely track the expected current curve without delay, and the waveforms of the two are close to overlapping	During the period from 0.12 s to 0.3 s, the wrist joint rotated at a certain angle, and the expected speed of the motor changed from 0 to 475 rpm, then decreased to −175 rpm, and stabilized around 0 after 0.32 s. During the period of 0.12 s to 0.3 s, the actual speed of the motor increased from 0 to 240 rpm and then decreased to −20 rpm, stabilizing around 0 after 0.25 s.	At 0.12 s, the expected position of wrist joint movement suddenly changed from 0 to 1000, and the actual motion position continued to rise during the period of 0.12 s to 0.18 s. Around 0.18 s, the actual position showed supersimulation, and the expected position was tracked at 0.3 s.

. Table 6 analyzes the expected current and actual current of the wrist joint motor, the expected speed and actual speed of the motor, and the curves of the expected position and actual position of the wrist joint movement.

From the above actual test results, it can be seen that under the feedforward control, the wrist joint has good robustness to variable load conditions while ensuring position control accuracy. The control methods of feedforward control and position fusion not only have the fastest speed of feedforward control but also have the precise characteristics of feedback control. The controller has good dynamic response performance and anti-interference ability.

7. Conclusions

A robot arm with a folding manipulator is designed in this paper, and the correctness of the kinematics algorithm is verified by modeling, analysis, and simulation. A fusion method of speed feedforward and position control is proposed, which can be used to solve the control problem of manipulator joints. The closed-loop feedback adopts a PID controller to correct the error, and the speed feedforward compensates for the dynamic performance of the system. The simulation results show that the strategy has a fast dynamic response ability and can converge quickly under the premise of ensuring accuracy. The effectiveness of the simulation results is further verified by experiments. The results indicate that compared to traditional five-DOF robotic arm systems, the control system of this robotic arm has lower tracking delay and better dynamic response performance. It can improve the system response performance while also enhancing its anti-interference ability. The robot arm joint is smoother in the process of rotation and can meet the precision requirements of wrist motor control. The results show that the method can meet the operation requirements of the robot arm and provide a practical basis for realizing the trajectory motion of the robot arm in complex space.