Next Article in Journal
Multiple Performance Optimization for Microstrip Patch Antenna Improvement
Previous Article in Journal
Improving the Angular Visibility of Photopolymer-Based Reflection Holograms for Sensing Applications
Previous Article in Special Issue
Development of a Visual Perception System on a Dual-Arm Mobile Robot for Human-Robot Interaction
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

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

School of Mechanical Electronic & Information Engineering, China University of Mining and Technology (Beijing), Beijing 100083, China
*
Author to whom correspondence should be addressed.
Sensors 2023, 23(9), 4276; https://doi.org/10.3390/s23094276
Submission received: 12 March 2023 / Revised: 21 April 2023 / Accepted: 24 April 2023 / Published: 25 April 2023
(This article belongs to the Special Issue Advanced Sensors for Intelligent Control Systems)

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.
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:
t a n θ = x y
From the inverse trigonometric function, the elbow joint angle of the forearm is:
θ = a r c t a n x y
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 θ i . This is rotation transformation, which is represented by the rotation transformation operator [13]:
R o t z i 1 θ i = cos θ i sin θ i 0 sin θ i cos θ i 0 0 0 1
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:
T r a n s z i 1 d i = 1 0 0 0 0 1 0 0 0 0 1 d i 0 0 0 1
From Z i 1 Axis moves the link length of a i to Z i axis. Transform for translation:
T r a n s z i 1 a i = 1 0 0 a i 0 1 0 0 0 0 1 0 0 0 0 1
Around X i axis rotates clockwise the connecting rod angle α i , transform for rotation:
R o t z i 1 α i = 1 0 0 0 cos α i sin α i 0 sin α i cos α i
Then the general transformation matrix from {i−1} coordinate system to {i} coordinate system is:
T i i 1 = cos θ i sin θ i cos α i sin θ i sin α i a i cos θ i sin θ i cos θ i cos α i cos θ i sin α i a i sin θ i 0 sin α i cos α i d i 0 0 0 1
where T i 1 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:
T i 0 = T 1 0 T 2 1 T 3 2 T i i 1
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. Joint angle θ 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 α 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 θ 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, s i n θ i and c o s θ i is recorded as S i and C i . Add s i n ( θ m + θ n ) , c o s ( θ m + θ n ) and θ m + θ n recorded as S m n , C m n and θ m n , where i, m, n ∈ {0,1,2,3,4,5,6}.
T 1 0 = C 1 0 S 1 0 S 1 0 C 1 0 0 1 0 d 1 0 0 0 1
T 2 1 = 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
T 3 2 = 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
T 4 3 = 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
T 5 4 = 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
T 6 5 = C 6 S 6 0 0 S 6 C 6 0 0 0 0 1 d 6 0 0 0 1
We multiply the above formula to obtain the position and attitude of the center of the manipulator end actuator.
T 6 0 = T 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 = 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
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 = a 5 C 2345 + d 6 S 2345 + a 3 C 23 + a 2 C 2 + a 4 C 234 C 1 ;
p y = a 5 C 2345 + d 6 S 2345 + a 3 C 23 + a 2 C 2 + a 4 C 234 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 θ 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 θ 1
We multiply both sides of Formula (15) by the matrix T 1 0 1 , the following equation can be obtained:
T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 = T 1 0 1 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
The following matrix can be obtained by simplifying the right side of Equation (16):
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
The following matrix can be obtained by simplifying the left side of Equation (16):
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
We observe the matrix (17) and (18), and select the element in the fourth column of the third row to obtain a θ 1 and obtain the univariate linear Equation (19) [20],
p x S 1 p y C 1 = 0
It can be solved as follows:
θ 1 = a t a n 2 p y , p x
2.
We solve joint angle θ 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 θ 6 (21),
n x S 1 n y C 1 = S 6 o x S 1 o y C 1 = C 6
Since θ 1 is known, we can obtain:
θ 6 = a t a n 2 n x S 1 n y C 1 , o x S 1 o y C 1
3.
We solve joint angle θ 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 θ 2345 (23),
o x C 1 + o y S 1 = S 2345 o z = C 2345
Since θ 1 is known, we can obtain:
θ 2345 = a t a n 2 o x C 1 + o y S 1 , o z
4.
We solve joint angle θ 4
Since θ 4 is a virtual joint, so the joint angle value is fixed as:
θ 4 = p i / 2
5.
We solve joint angle θ 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 θ 23 and θ 234 (26)
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
We move the term of equation group (26), move the known term to the left, and the unknown term to the right, and θ 4 is known, so the trigonometric function identity transformation is used, and the equation set (27) can be obtained by simplifying the term of θ 4 :
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
We add the two sides of the equation set (4.25) after the square, which can be eliminated using the elimination method θ 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 = 2 a 2 a 3 C 3 2 a 2 a 4 S 3
In the formula:
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
Using the trigonometric identity transformation, it is assumed that:
η sin φ = a 3 η cos φ = a 4
Then:
η = a 3 2 + a 4 2 , φ = a t a n 2 a 3 , a 4
By introducing Equation (29) into Equation (28), we can obtain:
sin φ θ 3 = A 2 + B 2 a 2 2 a 3 2 a 4 2 2 a 2 η cos φ θ 3 = ± 1 A 2 + B 2 a 2 2 a 3 2 a 4 2 2 a 2 η 2
According to Equation (31):
θ 3 = a t a n 2 a 3 , a 4 a t a n 2 ( A 2 + B 2 a 2 2 a 3 2 a 4 2 , ± 4 a 2 2 a 3 2 + a 4 2 A 2 + B 2 a 2 2 a 3 2 a 4 2 )
6.
We solve joint angle θ 5
So far, the values of θ 1 , θ 3 , θ 4 , θ 6 , θ 2345 have been solved. We continue to observe the matrices (17) and (18), and the solution to obtain θ 5 continues to simplify Equation (16) and multiply both sides of Equation (16) by T 6 5 1 , T 5 4 1 , and the matrix can be obtained by simplifying the left side of the equation:
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
On the right side of the simplified Equation (16), we can obtain θ 5 . Because the matrix is too complex, only the second row of elements needed is listed here:
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
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 θ 5 .
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
At this point, the trigonometric function equation of θ 234 and θ 2345 are brought into the equation group (26), and the equation transformation is performed again to obtain the following formal equation group:
a 2 C 2 = P X S 5 + Y C 5 a 2 S 2 = Q X C 5 + Y S 5
In the formula:
N = n z C 6 o z S 6 ; P = ( a 5 q z + d 6 q x C 1 + q y S 1 p x C 1 p y S 1 ) ; Q = a 5 q x C 1 + q y S 1 + d 6 q z p z + d 1 ; X = a 3 q z + a 4 N ; Y = a 3 N a 4 q z
We use the elimination method to square and add the two sides of the equation group (36), which can be eliminated θ 2 relevant elements, the following equation is obtained:
2 Q Y P X S 5 2 Q X + P Y C 5 = a 2 2 P 2 Q 2 X 2 Y 2
Using the trigonometric identity transformation, it is assumed that:
η sin φ = 2 Q Y P X η cos φ = 2 Q X + P Y
Then:
η = 2 Q Y P X 2 + 2 Q X + P Y 2
φ = a t a n 2 2 Q Y P X , 2 Q X + P Y
By introducing Equation (38) into Equation (37), we can obtain:
θ 5 = a t a n 2 ± M P 2 + Q 2 + X 2 + Y 2 a 2 2 2 , P 2 + Q 2 + X 2 + Y 2 a 2 2 a t a n 2 2 Q Y P X , 2 P Y + Q X
In the formula:
N = n z C 6 o z S 6 ; P = ( a 5 q z + d 6 q x C 1 + q y S 1 p x C 1 p y S 1 ) ; Q = a 5 q x C 1 + q y S 1 + d 6 q z p z + d 1 ; X = a 3 q z + a 4 N ; Y = a 3 N a 4 q z ; M = 4 Q Y P X 2 + P Y + Q X 2
7.
We solve joint angle θ 2
According to the principle that joint angles can be vector added, we can obtain the joint angle of θ 2 :
θ 2 = θ 2345 θ 3 θ 4 θ 5
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 m , a 3 = 0.125 m , a 4 = 0.366 m , a 5 = 0.075 m ; Joint offset d 1 = 0.2 m , d 6 = 0.3 m . At the same time, we change the initial joint angle θ i . (i = 1,2,3,5,6) is assigned to 0, i.e., θ i = 0 ° , (i = 1,2,3,5,6), the virtual joint angle value is fixed, θ 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:
T 6 0 = 0 0 1 0.925 0 1 0 0 1 0 0 0.641 0 0 0 1
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 θ = 0 0 0 P i / 2 0 0 , 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. 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.

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 ψ x
where u x refers to phase voltage, subscript x refers to three-phase winding A, B, C, i x refers to phase current, e ψ x is phase-induced potential, R x is phase resistance, R a = R b = R c = R in three-phase symmetrical winding.
e ψ x = d ψ x d t
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:
ψ A = L A i A + M A B i B + M A C i C + ψ p m θ
In the formula, ψ p m ( θ ) is the permanent magnetic linkage of the turning chain of phase A winding, θ is the rotor position angle, L A is self-induction of phase A winding; M A B and M A C 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 = R i A + L M d i A d t + e A
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]:
u A B u B C u C A = R R 0 0 R R R 0 R i A i B i C + L M M L 0 0 L M M L M L 0 L M d d t i A i B i C + e A e B e C
Electromagnetic torque:
T e = e A i A + e B i B + e C i C Ω
Motor motion equation:
T e T L = J d ω d t + B v ω
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 ) = K P e ( k ) + T T I j = 0 k e ( j ) + T D T ( e ( k ) e ( k 1 ) )
u k = u k u k 1 = K P ( e ( k ) e ( k 1 ) ) + K I e ( k ) + K D ( e ( k ) 2 e ( k 1 ) + e ( k 2 ) )
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 ( k ) , 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.

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. 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.

Author Contributions

Conceptualization, J.Z. (Jianwei Zhao); data curation, J.Z. (Jiaxin Zhou); formal analysis, J.Z. (Jiaxin Zhou); funding acquisition, J.Z. (Jianwei Zhao); investigation, J.Z. (Jiaxin Zhou); methodology, J.Z. (Jiaxin Zhou); project administration, J.Z. (Jianwei Zhao); resources, J.Z. (Jiaxin Zhou); software, J.Z. (Jiaxin Zhou); supervision, J.Z. (Jianwei Zhao); validation, J.Z. (Jiaxin Zhou); visualization, J.Z. (Jiaxin Zhou); writing—original draft, J.Z. (Jiaxin Zhou); writing—review and editing, J.Z. (Jianwei Zhao). All authors have read and agreed to the published version of the manuscript.

Funding

This research was funded by the China Association of Higher Education, grant number 21GCYB04, National Social Science Foundation of China, grant number BIA200191, National Natural Science Foundation of China, grant number 51575005, and The Engineering Research Center of the Ministry of Education 2022 Annual Innovation Fund Project—Research on the Teaching System of Digital Virtuality and Educational Robot for Robot Engineering Specialty, project approval No. 1221051.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

The data are available upon request.

Conflicts of Interest

The authors declare no conflict of interest.

References

  1. Ruangpayoongsak, N.; Roth, H.; Chudoba, J. Mobile robots for search and rescue. In Proceedings of the IEEE International Safety, Security and Rescue Rototics, Workshop, Kobe, Japan, 6–9 June 2005; pp. 212–217. [Google Scholar] [CrossRef]
  2. Li, J.; Dong, T.; Li, Y. Research on task allocation in multiple logistics robots based on an improved ant colony algorithm. In Proceedings of the IEEE 2016 International Conference on Robotics and Automation Engineering (ICRAE), Jeju, Republic of Korea, 27–29 August 2016. [Google Scholar]
  3. Zhang, X.Y.; Sun, J.Q.; Ding, Q. Kinematics analysis and time delayed control of a five-degree of freedom manipulator. Hsi-Chiao Tung Ta Hsueh/J. Xi’an Jiaotong Univ. 2013, 47, 103–108. [Google Scholar] [CrossRef]
  4. Gao, J.Y.; Gao, X.S.; Wei, B.Y. Heavy Explosive Removing Robot Control Technique Research. In Proceedings of the 2009 International Conference on Intelligent Human-Machine Systems and Cybernetics, Hangzhou, China, 26–27 August 2009; pp. 85–89. [Google Scholar] [CrossRef]
  5. Sariyildiz, E.; Sekiguchi, H.; Nozaki, T.; Ugurlu, B.; Ohnishi, K. A Stability Analysis for the Acceleration-Based Robust Position Control of Robot Manipulators via Disturbance Observer. IEEE/ASME Trans. Mechatron. 2018, 23, 2369–2378. [Google Scholar] [CrossRef]
  6. Wang, X.S.; Zhang, Q.; Shen, X.P.; Li, J. Control of Tendon-Sheath Actuated Slender Manipulator. IEEE Trans. Control Syst. Technol. 2020, 28, 688–696. [Google Scholar] [CrossRef]
  7. Wu, S.; Dong, B.; Ding, G.B.; Wang, G.G.; Liu, G.J.; Li, Y.C. Backstepping sliding mode force/position control for constrained reconfigurable manipulator based on extended state observer. In Proceedings of the 12th World Congress on Intelligent Control and Automation (WCICA), Guilin, China, 12–15 June 2016; pp. 477–482. [Google Scholar] [CrossRef]
  8. Xing, Y.; Yang, Y.P.; Zu, Q.; Yu, J.J. Application of AGV technology and design and calculation of driving system. AIP Conf. Proc. 2019, 2073, 020028. [Google Scholar]
  9. Tang, T.; Huang, Y.M. Combined line-of-sight error and angular position to generate feedforward control for a charge-coupled device–based tracking loop. Opt. Eng. 2015, 54, 105107. [Google Scholar] [CrossRef]
  10. Liu, W.H.; Dai, J.X. Design of Attitude Sensor Acquisition System Based on STM32. In Proceedings of the 2015 Fifth International Conference on Instrumentation and Measurement Computer Communication and Control (IMCCC), Qinhuangdao, China, 18–20 September 2015; pp. 1850–1853. [Google Scholar]
  11. Webster, D. A Pulsed Ultrasonic Distance Measurement System Based upon Phase Digitizing. IEEE Trans. Instrum. Meas. 1994, 43, 578–582. [Google Scholar] [CrossRef]
  12. Guo, F.Y.; Cai, H.; Ceccarelli, M. Enhanced DH: An improved convention for establishing a robot link coordinate system fixed on the joint. Ind. Robot-Int. J. Robot. Res. Appl. 2020, 47, 197–205. [Google Scholar] [CrossRef]
  13. Sun, J.D.; Cao, G.Z.; Li, W.B. Analytical Inverse Kinematic Solution Using the D-H Method for a 6-DOF Robot. In Proceedings of the 14th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI), Jeju, Republic of Korea, 28 June–1 July 2017. [Google Scholar]
  14. Liu, T.L. Motion Planning and Control of Cable-Driven Segmented Linkage Manipulators for Narrow Space Operations. Ph.D. Thesis, Harbin Institute of Technology, Harbin, China, 2021. [Google Scholar]
  15. AlAttar, A.; Cursi, F.; Kormushev, P. Kinematic-Model-Free Redundancy Resolution Using Multi-Point Tracking and Control for Robot Manipulation. Appl. Sci. 2021, 11, 4746. [Google Scholar] [CrossRef]
  16. Zhang, S.Y.; Dai, S.L.; Zanchettin, A.M.; Villa, R. Trajectory planning based on non-convex global optimization for serial manipulators. Appl. Math. Model. 2020, 84, 89–105. [Google Scholar] [CrossRef]
  17. Palmieri, G.; Scoccia, C. Motion Planning and Control of Redundant Manipulators for Dynamical Obstacle Avoidance. Machines 2021, 9, 121. [Google Scholar] [CrossRef]
  18. Jiang, H.H.; Jin, X.Y.; Xing, X.Y.; Mei, G.F.; Wu, P. Five-degree-of- freedom Manipulator Trajectory Planning Based on PSO Particle Algorithm. Mach. Des. Res. 2020, 36, 107–110. [Google Scholar]
  19. Chen, S.; Luo, M.Z.; Abdelaziz, O. A general analytical algorithm for collaborative robot (cobot) with 6 degree of freedom (DOF). In Proceedings of the IEEE International Conference on Applied System Innovation (ICASI), Sapporo, Japan, 13–17 May 2017; pp. 698–701. [Google Scholar]
  20. Xiong, W.; Chen, Z.T.; Wu, H.; Xu, G.; Mei, H.P.; Ding, T.; Yang, Y. Fetching trajectory planning of explosive ordnance disposal robot. Ferroelectrics 2018, 529, 67–79. [Google Scholar] [CrossRef]
  21. Shin, W.G.; Lee, S.H. An analysis of the main factors on the wear of brushes for automotive small brush-type DC motor. J. Mech. Sci. Technol. 2010, 24, 37–41. [Google Scholar] [CrossRef]
  22. Xia, C.; Fang, H. Permanent magnet brushless DC motor drives and controls. Diangong Jishu Xuebao/Trans. China Electrotech. Soc. 2012, 27, 25–34. [Google Scholar]
  23. Tan, X.P.; Li, X.B.; Xiao, T.L. Real-time analysis of dynamic priority of CAN bus protocol. In Proceedings of the 2010 International Conference on Electronics and Information Engineering, Kyoto, Japan, 1–3 August 2010; Volume 1, pp. 219–222. [Google Scholar] [CrossRef]
  24. Kong, H.; Liu, J.; Cui, G. Study on Field-Weakening Theory of Brushless DC Motor Based on Phase Advance Method. In Proceedings of the 2010 International Conference on Measuring Technology and Mechatronics Automation, Changsha, China, 13–14 March 2010; pp. 583–586. [Google Scholar] [CrossRef]
  25. Meng, Z.J.; Chen, R.; Sun, C.Z.; An, Y.J. The mathematical simulation model of brushless dc motor system. In Proceedings of the 2010 International Conference on Computer Application and System Modeling (ICCASM 2010), Taiyuan, China, 22–24 October 2010; Volume 12, pp. 625–629. [Google Scholar] [CrossRef]
  26. Chen, J.; Guo, Y.; Zhu, J.; Jin, J. Performance Analysis of a Surface Mounted Permanent Magnet Brushless DC Motor Using an Improved Phase Variable Model. In Proceedings of the 2007 IEEE Industry Applications Annual Meeting, New Orleans, LA, USA, 23–27 September 2007; pp. 2169–2174. [Google Scholar] [CrossRef]
  27. Jing, X.; Cheng, L. An Optimal PID Control Algorithm for Training Feedforward Neural Networks. IEEE Trans. Ind. Electron. 2013, 60, 2273–2283. [Google Scholar] [CrossRef]
  28. Alvarez-Ramirez, J.; Cervantes, I.; Kelly, R. PID regulation of robot manipulators: Stability and performance. Syst. Control Lett. 2000, 41, 73–83. [Google Scholar] [CrossRef]
  29. Benotsmane, R.; Kovács, G. Optimization of Energy Consumption of Industrial Robots Using Classical PID and MPC Controllers. Energies 2023, 16, 3499. [Google Scholar] [CrossRef]
  30. Tsai, C.C.; Cheng, M.B.; Chan, C.K. Adaptive backstepping position control for a linear DC brushless motor. In Proceedings of the 30th Annual Conference of IEEE Industrial Electronics Society, 2004, IECON 2004, Busan, Republic of Korea, 2–6 November 2004; Volume 1, pp. 365–370. [Google Scholar] [CrossRef]
  31. Kim, T.H.; Lee, H.W.; Ehsani, M. State of the art and future trends in position sensorless brushless DC motor/generator drives. In Proceedings of the 31st Annual Conference of IEEE Industrial Electronics Society, 2005, IECON 2005, Raleigh, NC, USA, 6–10 November 2005; pp. 1718–1725. [Google Scholar] [CrossRef]
  32. Kelly, R. A Linear-State Feedback Plus Adaptive Feed-Forward Control for DC Servomotors. IEEE Trans. Ind. Electron. 1987, IE–34, 153–157. [Google Scholar] [CrossRef]
  33. Anuchin, A.; Kozachenko, V. Current loop dead-beat control with the digital PI-controller. In Proceedings of the 2014 16th European Conference on Power Electronics and Applications, Lappeenranta, Finland, 26–28 August 2014; pp. 1–8. [Google Scholar] [CrossRef]
Figure 1. Five-degree-of-freedom manipulator system diagram.
Figure 1. Five-degree-of-freedom manipulator system diagram.
Sensors 23 04276 g001
Figure 2. Structure diagram of mechanical arm folding state: 1. Mechanical Claw; 2. Wrist Joint; 3. Elbow Joint of Forearm; 4. Middle Arm Elbow; 5. Shoulder Joint 2; 6. Shoulder Joint 1; 7. Mechanical body.
Figure 2. Structure diagram of mechanical arm folding state: 1. Mechanical Claw; 2. Wrist Joint; 3. Elbow Joint of Forearm; 4. Middle Arm Elbow; 5. Shoulder Joint 2; 6. Shoulder Joint 1; 7. Mechanical body.
Sensors 23 04276 g002
Figure 3. (a) Working principle of ultrasonic sensors; (b) Angle diagram of forearm elbow joint.
Figure 3. (a) Working principle of ultrasonic sensors; (b) Angle diagram of forearm elbow joint.
Sensors 23 04276 g003
Figure 4. D-H method for establishing connecting rod coordinate system.
Figure 4. D-H method for establishing connecting rod coordinate system.
Sensors 23 04276 g004
Figure 5. Structure diagram of the mechanical arm after introducing the virtual joint.
Figure 5. Structure diagram of the mechanical arm after introducing the virtual joint.
Sensors 23 04276 g005
Figure 6. Flow chart of forward kinematics analysis and verification of 5-DOF folding manipulator.
Figure 6. Flow chart of forward kinematics analysis and verification of 5-DOF folding manipulator.
Sensors 23 04276 g006
Figure 7. MATLAB Robotic Toolbox to solve forward kinematics calculation results.
Figure 7. MATLAB Robotic Toolbox to solve forward kinematics calculation results.
Sensors 23 04276 g007
Figure 8. Kinematic model of the manipulator in the initial state.
Figure 8. Kinematic model of the manipulator in the initial state.
Sensors 23 04276 g008
Figure 9. Flow chart of inverse kinematics analysis and verification of 5-DOF folding manipulator.
Figure 9. Flow chart of inverse kinematics analysis and verification of 5-DOF folding manipulator.
Sensors 23 04276 g009
Figure 10. Ground station control system: 1. Data transmission antenna; 2. Button of mechanical claw; 3. Power switch; 4. Button of elbow joint; 5. Button of forearm elbow joint; 6. Display screen; 7. Power indicator; 8. Image transmission antenna; 9. Button of middle arm elbow joint; 10. Shoulder joint 2 button; 11. Shoulder joint 1 button.
Figure 10. Ground station control system: 1. Data transmission antenna; 2. Button of mechanical claw; 3. Power switch; 4. Button of elbow joint; 5. Button of forearm elbow joint; 6. Display screen; 7. Power indicator; 8. Image transmission antenna; 9. Button of middle arm elbow joint; 10. Shoulder joint 2 button; 11. Shoulder joint 1 button.
Sensors 23 04276 g010
Figure 11. Wiring diagram of motor and driver: 1. Power supply; 2. Driver; 3. DC brushless motor.
Figure 11. Wiring diagram of motor and driver: 1. Power supply; 2. Driver; 3. DC brushless motor.
Sensors 23 04276 g011
Figure 12. Overall control process of the robotic arm.
Figure 12. Overall control process of the robotic arm.
Sensors 23 04276 g012
Figure 13. Control block diagram of wrist joint motor.
Figure 13. Control block diagram of wrist joint motor.
Sensors 23 04276 g013
Figure 14. Block diagram of the current loop.
Figure 14. Block diagram of the current loop.
Sensors 23 04276 g014
Figure 15. Speed loop block diagram.
Figure 15. Speed loop block diagram.
Sensors 23 04276 g015
Figure 16. Position ring block diagram.
Figure 16. Position ring block diagram.
Sensors 23 04276 g016
Figure 17. Current simulation diagram of wrist joint motor.
Figure 17. Current simulation diagram of wrist joint motor.
Sensors 23 04276 g017
Figure 18. Simulation diagram of wrist joint motor speed.
Figure 18. Simulation diagram of wrist joint motor speed.
Sensors 23 04276 g018
Figure 19. Simulation diagram of wrist joint motor position.
Figure 19. Simulation diagram of wrist joint motor position.
Sensors 23 04276 g019
Figure 20. Wrist joint loading diagram.
Figure 20. Wrist joint loading diagram.
Sensors 23 04276 g020
Figure 21. (a) Expected current and actual current curve of wrist joint motor; (b) Expected speed and actual speed curve of wrist joint motor; (c) Expected position and actual position curve of wrist joint movement.
Figure 21. (a) Expected current and actual current curve of wrist joint motor; (b) Expected speed and actual speed curve of wrist joint motor; (c) Expected position and actual position curve of wrist joint movement.
Sensors 23 04276 g021
Figure 22. (a) Expected current and actual current curve of wrist joint motor; (b) Expected speed and actual speed curve of wrist joint motor; (c) Expected position and actual position curve of wrist joint movement.
Figure 22. (a) Expected current and actual current curve of wrist joint motor; (b) Expected speed and actual speed curve of wrist joint motor; (c) Expected position and actual position curve of wrist joint movement.
Sensors 23 04276 g022
Table 1. Range of motion of manipulator joint.
Table 1. Range of motion of manipulator joint.
Mechanical ClawWrist JointElbow 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°
Table 2. D-H parameter table of manipulator after introducing virtual joint.
Table 2. D-H parameter table of manipulator after introducing virtual joint.
Joint d i a i α i θ i Joint Range/°
Shoulder joint 1 d 1 0Pi/2 θ 1 0~180
Shoulder joint 20 a 2 0 θ 2 −60~180
Middle arm elbow0 a 3 0 θ 3 −120~90
Virtual joint0 a 4 0 θ 4 90
Elbow joint of forearm0 a 5 Pi/2 θ 5 −90~90
Wrist joint d 6 00 θ 6 −180~180
Table 3. Four groups of inverse kinematics solutions.
Table 3. Four groups of inverse kinematics solutions.
Order NumberJoint θ / r a d Joint θ / r a d
1Shoulder joint 11.0472Shoulder joint 20.9839
Middle arm elbow−1.0472Virtual joint1.5708
Elbow joint of forearm0.5869Wrist joint1.5708
2Shoulder joint 11.0472Shoulder joint 21.0472
Middle arm elbow−1.0472Virtual joint1.5708
Elbow joint of forearm0.5236Wrist joint1.5708
3Shoulder joint 11.0472Shoulder joint 21.1038
Middle arm elbow−1.1671Virtual joint1.5708
Elbow joint of forearm0.5869Wrist joint1.5708
4Shoulder joint 11.0472Shoulder joint 21.1671
Middle arm elbow−1.1671Virtual joint1.5708
Elbow joint of forearm0.5236Wrist joint1.5708
Table 4. The relevant parameters and interface functions of the controller.
Table 4. The relevant parameters and interface functions of the controller.
Basic ParametersNumerical or Interface Functions
MCUSTM32F103
Communication protocolCAN bus protocol
Baud rate500 K Bit/s
Sampling time point1.75 μs
CAN interfaceUsed for communication of functional testing
JTAG interfaceUsed for DAP online simulation and program download
Table 5. Simulation parameters.
Table 5. Simulation parameters.
Basic ParametersNumerical Value
Proportional gain K p 200
Integral term K I 3.3
Differential term K D 2.2
Simulation time T0.3 s
Table 6. Experimental results of current, speed, and position under no-load and loaded conditions.
Table 6. Experimental results of current, speed, and position under no-load and loaded conditions.
Wrist JointMotor CurrentMotor SpeedWrist Joint Position
No loadDuring 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 loadDuring 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.
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

Share and Cite

MDPI and ACS Style

Zhao, J.; Zhou, J. The Five-DOF Explosion-Removal Manipulator Based on Position and Velocity Feedforward Compensation Control. Sensors 2023, 23, 4276. https://doi.org/10.3390/s23094276

AMA Style

Zhao J, Zhou J. The Five-DOF Explosion-Removal Manipulator Based on Position and Velocity Feedforward Compensation Control. Sensors. 2023; 23(9):4276. https://doi.org/10.3390/s23094276

Chicago/Turabian Style

Zhao, Jianwei, and Jiaxin Zhou. 2023. "The Five-DOF Explosion-Removal Manipulator Based on Position and Velocity Feedforward Compensation Control" Sensors 23, no. 9: 4276. https://doi.org/10.3390/s23094276

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop