Design and Control of a Firefight Cannon Manipulator Applying Sliding Mode Control

2.1 Mechanical structure

The FCM is designed to perform the tasks of fire extinguishing and is composed of an elbow, swivel and nozzle. The system includes two motors (motors 1 and 2), which are used for the pan and tilt motion and the motor control nozzle are used for fog-stream. Each motor is composed of a reduction gear, DC motor and encoder. One important point in the FCM is that the gears applied are worm gears. They are used to prevent backward rotation from the reaction of water shooting.

The structures of the manipulator are composed as follows: The manipulator joints are composed of two degrees of freedom with yawing motion of the first axis and pitching motion of the second axis, as shown in Figure 1 and 2. The first joint actuator is designed to move around in a range between 0° and 360°. The second joint actuator is in a range from -10° to 185°.

Figure 2:

Joints of the FCM

The weights of the two links are 7.55 kg and 4.06 kg each. The total weight of the manipulator system is approximately 11.61 kg. The manipulator has been developed to cope with a maximum load capacity of 123 kg.

Table 1 lists the specification of the measured and calculated FCM and Table 2 and 3 present the parameters of motors and inertial constants.

Table 1

Parameters of the firefight cannon manipulator

Table 2

Parameters of the motors

Table 3

Inertial Constants (kgm2 )

2.2 Control system

A control system has been developed to control the FCM, and the entire control system consists of a main controller, and joint motor controller, as shown in Figure 3. The main controller based on an ARM processor plays the role of editing the control algorithm, sending the order of the operation signals to the motor controller for the robot joint actuators through RS232 communication. The main controller sends continuous motion signals to motor controllers of the joint actuators according to a trajectory planning. For the joint motion controller, a STM32F207 microprocessor is applied. The controller is built in a modular control board capable of controlling two motors and has been designed to generate the Pulse Width Modulation (PWM) signal and the direction signal for the motor and to send them from the main controller to the motor driver. The motor driver has been developed to supply the power, which is proportionally amplified with the duty ratio to the joint actuating motors according to the PWM signal and the direction signal for the joint motor. Figure 4 shows a picture of water-shooting of FCM during according to control input.

A proportional-derivative and feed-forward control algorithm is planned to apply to the joint controller of the developed system.

Figure 3:

The developed control system

Figure 4:

The water-shooting of the FCM

3.1 Forward Kinematics analysis

An analysis of the kinematics of FCM was performed on the joint actuator links based on a two-axis of FCM. This is because the one-axis of the manipulator is fixed on the ground surface, as the reference axis. The coordinates have been set up using the D-H (Denavit - Hartenberg) convention, as shown in Figure 5, and Tables 4 lists the corresponding joint link parameters of a_i, α_i, d_i, and θ_i Each joint link is expressed as a homogeneous transformation matrix, A_i, by multiplying the two transformation matrices. The matrix of end - effector are obtained:

Figure 5:

D-H coordinates of 2-axis manipulator

Table 4

Denavit - Hartenberg

3.2 Inverse Kinematics analysis

Kinematic analysis was performed on joint actuator links based on a 2-axis manipulator. Since the 1-axis of manipulator was fixed on the ground surface, it was established as a reference axis. Coordinates were set up using the D-H (Denavit- Hartenberg) convention as shown in Figure 5, and then corresponding joint link parameters of a_i, α_i, d_i, and θ_i (where a_i is called the length, α_i is called the twist, d_i is called the offset, and θ_i is called the angle, respectively) were obtained. The following Equation (1) is derived after matrix transformation through consecutive multiplication. The following Equation (1) is derived after matrix transformation through consecutive multiplication.

The following Equation (2) is obtained from Equation (1).

Similarly, the following equations are obtained.

4.1 Modeling of water-shooting pump

In this study, a disturbance of the system is expressed as an external force. For a control volume (CV), the second link of a system where the water move out is enclosed. The only mass flow across the CV is water moving to the right. Figure 6 shows the CV of second link.

Figure 6:

Control volume of the second link

The force balance is:

where, $$ {\dot{m}}_{out}$$ is the mass flow, v_out is maximum velocity, respectively. And the mass flow $$ {\dot{m}}_{out}$$ is given by Equation (5):

With A_out, ρ and D are section area, water density and diameter of end-effector, respectively.

The final equation of water reaction force of FCM system is:

For the FCM system to work, it needs a water pump that can deliver a large volume of water at high pressure through a hoselike tube. This water pump will change the velocity of the water when it moves out.

In this study, maximum velocity v_out = 14m / s of water has been chosen to demonstrate the robustness of SMC controller. The diameter of the shooting nozzle is D = 7cm and the water density in this case is ρ = 1000(kg/m³) . Therefore, the maximum disturbance force is 754.3(N).

4.2 Dynamics of 2 DOF FCM

In the following sections, the 2 DOF FMC dynamic equations of motion can be concisely expressed concisely as:

where, q: 2x1 is position vector, τ : 2x1 is vector of torques, M(q) : 2x2 is inertia matrix of the manipulator, $$ C\left(q,\dot{q}\right)$$ ): 2x2 is vector of Centrifugal and Coriolis terms, G(q): 2x1 is vector of gravity terms, $$ J_E^T$$ is Jacobian matrix and _F is external forces, respectively.

Matrix M and C are symmetric 2x2 matrices:

The inertia matrix is given by Equation (8),:

where m_i,i=_1,2 is the mass of link each, J_Ti and , J_Ri represent the Jacobian of linear velocity an angular velocity of link each, respectively.

While the Centrifugal and Coriolis matrix $$ C\left(q,\dot{q}\right)$$) is given by Equation (9):

Matrix G is given by Equation (10):

where, Π is the potential energy of the manipulator, which is just the sum of those of the two links. For each link, the potential energy is just its mass multiplied by the gravitational acceleration and the height of its center of mass.

By starting with the end conditions of f_n+1 = 0 , τ_n+1 = 0 . The torques τ_i of each joint are calculated consecutively. The detailed expression of Equation (8), (9), (10), and the torques τ_i are described in Appendix.

5.1 Design PD Controllers and Computer Simulation

The SMC and PD controller have been designed to track the desired trajectories of a vehicle and joints. A PD control algorithm is applied to control the position each link of system. The PD control laws for each link are described in Equation (11).

where e_i = x_di - x_i, $$ {\dot{e}}_i$$ = $$ {\dot{x}}_{di}$$ - $$ {\dot{x}}_i$$, K_pi and K_di denote a proportional and derivative gain, respectively. The gains used for the PD controller are K_p = 90000, K_d = 600, respectively. Figure 7 represents a block diagram of the PD controller with a disturbance.

Figure 7:

Block diagram of PD controller with a disturbance

In the first simulation, the PD controller is applied to the system. The trajectory of the end-effector is assigned to move from the initial position to the target position with a constant velocity for 10 seconds. Figure 8, 9, 10 and 11 present the simulation results. Figure 8 and 9 are the simulation results of manipulator without a disturbance. Figure 10 and 11 show the simulation results under a disturbance. The simulation shows that the measured trajectory is tracking the desired trajectory with good performance. Figure 9 and 11 show that the errors of two joints are quite small (i.e., without disturbance, the error of the first a second joint is 0° and approximately 7 10^-3°. On the other hand, under the disturbance of a water reaction, the error of first joint varies from -0.01° to 0.01° and second joint is varied from 0.005° to 0.014°).

Figure 8:

Joint angles (PD control without a disturbance)

Figure 9:

Joint torques and position errors of the joints (PD control without a disturbance)

Under a disturbance, the input torques are larger than that without a disturbance (without disturbance, the input torques of the first and second link are approximately 11Nm and 0.3Nm, respectively, and under disturbance, the input torques of the first and second link are approximately 20Nm and 18Nm, respectively), as shown in Figure 9 and 11.

Figure 10:

Joint angles (PD control with a disturbance)

Figure 11:

Joint torques and position errors of joints (PD control with a disturbance)

5.2 Design of the SMC Controllers and Computer Simulation

The sliding mode method is based on the idea of keeping the scalar quantity, s, which is a weighted sum of the position error (q - q_d), the velocity error($$ \dot{q}$$ - $$ {\dot{q}}_d$$, and (not required) the acceleration error($$ \stackrel{\text{̇}}{q}$$ - $$ {\stackrel{\text{̇}}{q}}_{\mathrm{d}}$$), at zero as reported elsewhere [6]. Here, the expression of s is chosen as expressed in Equation (12):

where λ > 0 is the weight parameter.

Therefore, the task of the controller is to take s to zero. When s approaches zero, the position error (and velocity error, also) also approaches zero too. Therefore, and thus, trajectory tracking is performed. Once s is zero, to keep it at this value, the derivative of s is expected to be zero. From Equation (12), the expression of s $$ \dot{S}$$ can be deduced easily as follows:

Substituting the expression of $$ \ddot{q}$$ deduced from Equation (7) into Equation (13):

So, if derivative of the scalar quantity $$ \dot{S}$$ = 0, we have:

In addition, the actual control law, which can be robust to uncertainties, is the function chosen as follows:

where, sat(.) is the saturating function, ㅣsat(.)ㅣ≤ 1, φ > 0 is the boundary layer thickness, K is the design parameter chosen so that $$ \dot{s}$$ ≤ -η ㅣsㅣ < 0, with η is a strictly positive constant.

The parameters, φ, λ and K used for the SMC controller are φ=1, λ = [800; 800]^T and K = [500; 400]^T, respectively. To verify the good performance of the designed SMC, the trajectories were have been created for links and the simulation of trajectory tracking was has been carried out. Figure 12 presents the block diagram of SMC with a disturbance.

Figure 12:

A block diagram of the SMC with a disturbance

In the second simulation, the SMC was applied to the system. The trajectory of the end-effector was assigned to move from the initial position to the target position with a constant velocity for 10 seconds. Figure 13 - 16 present the simulation results. Figure 13 and 14 show simulation results of the manipulator without a disturbance. Figure 15 and 16 present the simulation results under disturbance. The simulation result shows that the measured trajectory is tracking the desired trajectory with good performance. Each joint angle is still controlled well with a disturbance. The simulation results show the validity of the SMC algorithms. Figure 14 and 16 show that the errors of the two joints are very small (i.e., without a disturbance, the error of the first and second joint is 0° and from -0.25×10^-5 degrees to 0.25×10^-5 degree. On the other hand, under a disturbance of the water reaction, the error of first joint varies from -2.2×10^-3 degrees to -2.2×10^-3 degrees and that of the second joint varies from -1.87×10^-3 degree to -1.87×10^-3 degree).

Figure 13:

Joint angles (SMC without a disturbance)

Figure 14:

Joint torques and position errors of joints (SMC without a disturbance)

Figure 15:

Joint angles (SMC with a disturbance)

Under a disturbance, the input torques are larger than that without a disturbance case (without a disturbance, the input torques of the first and second link are approximately 11Nm and 0.3Nm, respectively, and under a disturbance, the input torques of the first and second link are approximately 20Nm and 18Nm, respectively), as shown in Figure 14 and 16.

Figure 16:

Joint torques and position errors of joints (SMC with a disturbance)

5.3 Analyzed quality of the controller based on combining the data between the PD Controllers and SMC

The performance of the PD Controller and SMC have been compared. Figure 17 and 18 show the simulation results of the PD Controller and SMC Controller for both cases without a disturbance and under the presence of a disturbance. According to the results, the SMC shows good trajectory tracking along the target joint angle.

Figure 17:

Position errors of the joints (PD control and SMC without a disturbance)

Without a disturbance, the errors of the joints are very small, as shown in Figure 17. Under a disturbance, the simulation results show that the position errors are larger than the case without, as shown in Figure 18. Based on the results of comparing the performance of the SMC and PD controllers, the superior performance of the SMC in reducing the position errors is confirmed.

Figure 18:

Position errors of the joints (PD control and SMC with a disturbance)