- Email: [email protected]
position control of a compliant rescue manipulator
Mechatronics 46 (2017) 143–153
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
Development and hybrid force/position control of a compliant rescue manipulatorR Qingcong Wu a,∗, Xingsong Wang b, Bai Chen a, Hongtao Wu a, Ziyan Shao a a b
College of Mechanical and Electrical Engineering, Nanjing University of Aeronautics and Astronautics, Jiangsu Province 210016, China College of Mechanical Engineering, Southeast University, Jiangsu Province 211100, China
a r t i c l e
i n f o
Article history: Received 23 December 2016 Revised 13 May 2017 Accepted 3 August 2017
Keywords: Rescue robot Compliant gripper Self-sensing calibration Model identification Hybrid force/position control
a b s t r a c t Performing search and rescue tasks in the ruins after disasters demand rescue robots with slender and compliant structure to accommodate the complicated configurations under debris. This paper presents the structural design and system composition of a novel tendon-sheath actuated compliant rescue manipulator with slender and flexible body. The proposed robot can drill into the narrow space where rescuers and traditional rigid robots cannot get in because of size limitation or toxic environment. The self-sensing calibration, dynamic modeling, and hybrid force/position control trajectory of the compliant gripper with integrated position and force monitoring capabilities are analyzed and discussed. With the aim of regulating the gripper displacement and clamping force during operation, a hybrid force/position control strategy is proposed based on a cascaded proportional-integral-derivative (PID) controller and a fuzzy sliding mode controller (FSMC). Experimental setups mainly consisting of servo motor, tendon sheath transmission components, compliant gripper, and real-time control system are established to calibrate the strain gauge sensors and identify the dynamic model parameters. Further experimental investigations involving force tracking experiments, position tracking experiments, and object grasping experiments are carried out. The experimental results demonstrate the effectiveness of the developed self-sensing approach and control strategies during rescue operation. © 2017 Elsevier Ltd. All rights reserved.
1. Introduction In the last decade, large numbers of people have suffered from different kinds of natural or man-made disasters causing large scale of damages, such as earthquake, landslide, mining accidents, floods, and so on. After these disasters, it is of great significant to search the survivors in the ruins and provide necessary medical assistance and treatment as soon as possible. Existing researches indicate that the difficulty and risk of disaster rescue will increase rapidly after the so-called golden seventy-two h [1]. The surface survivors can be easily found and rescued in time. However, for the interior victims trapped inside the confined spaces under rubble piles, it is difficult and dangerous for the responders and canine teams to execute the rescue and relief works due to the harsh conditions. Therefore, more and more researchers have focused their efforts on developing rescue robots to explore deep under debris [2,3].
R ∗
This paper was recommended for publication by Associate Editor Roger Dixon. Corresponding author. E-mail address: [email protected] (Q. Wu).
http://dx.doi.org/10.1016/j.mechatronics.2017.08.003 0957-4158/© 2017 Elsevier Ltd. All rights reserved.
Most of the existing rescue robots were designed with smart size, multiple joints, and strong crawling capacity to drill into small crevices and access victims. Arai et al. [4,5] developed a series of snake-like crawler vehicles called Souryu for in-rubble searching operation. These robots were equipped with motors, cameras, sensors, battery and control unit inside the body and remote-operated via wireless joystick. The large cross section area definitely limited its mobility within narrow space. Kitagawa et al. [6] investigated a small-size rescue robot, named Active Hose, for searching the survivors within collapses. The Active Hose, made up of eccentric wire mechanism, was driven by several wound tube actuators and capable of curving to arbitrary direction. The major drawback of this rescue system is the large friction between robot body and external environment. A rescue manipulator with duplex parallel mechanism was designed by Hirayama and Kazuyukilto [7] in Hosei University to search narrow space for victims. This robot needs to be operated manually by rescuer with the consideration that electrical power supply might not be obtained in practical disaster situation. Chen and Wang [8] have designed a researching robot, composed of modular rigid steering head and flexible body, to work within small voids of rubble and confined space. Preliminary experimental results showed that the rigidity
144
Q. Wu et al. / Mechatronics 46 (2017) 143–153
property of steering head reduced the effectiveness in searching the interior through a rubble channel. Because of space limitations, traditional rescue robots with rigid body have difficulty in adapting to the complicated configurations under debris and executing rescue actions. Besides, in certain cases the end-effector of rescue robot needs to provide necessary assistance to the weaken victims. The compliance control technology, such as impedance control [9], admittance control [10] and hybrid force/position control [11], can be applied to ensure human-robotenvironment interactive safety during rescue operation [12]. Generally, the stiff joints of rigid manipulator need to be equipped with additional nonintegrated force/torque sensors and position sensors, such as capacitance sensors, inductance sensors, laser sensors, and optical reflective sensors, to acquire required feedback information and achieve desired compliance [13,14]. It leads to larger robot dimension and heavier weight and, thus, is not suitable to the miniature robot applications. Integrating the resistive strain gauge sensors with robotic system is an effective measuring method to achieve self-sensing capacity without additional commercial sensors, and it has been applied in many electromechanical systems [15]. Yang et al. [15,16] developed a piezo-driven gripper based on parallelogram mechanism and double-rocker mechanism. Two groups of strain gauges are attached to the flexure hinges and beams to measure the position and the gripping force. Xu [17] designed a compact compliant stage with a large rotational range by utilizing multistage compound radial flexures. The driving torque acting on the stage and the resulting rotational angle were detected via a strain-gauge sensor attached to the leaf flexure. Kam et al. [18] proposed a new strain gauge-based sensing scheme to measure the displacement of ionic polymer-metal composite actuators for water applications. Taking the above into account, in this paper a brand-new slender robot with a flexible body and a compliant manipulator is developed for disaster research and rescue. Firstly, the mechanical structure of the compliant manipulator is described, which consists of a rotary joint, two bending joints and a compliant gripper. The position and clamping force of the gripper can be detected by several strain gauges sensors attached at the beams with large strain. And then, a hybrid force/position control scheme is developed to regulate both gripper tip position and clamping force and, furthermore, achieve smooth interaction. The force control is realized via a cascaded proportional-integralderivative (PID) controller, whereas the position control is realized via a fuzzy sliding mode controller (FSMC). Finally, the effectiveness of the presented mechanical structural design, sensing strategy and control algorithm are demonstrated by trajectories tracking experiments and closing-clamping-opening operation of gripper. 2. Overview of the rescue robot system The overall architecture of the developed rescue robot system is depicted in Fig. 1. The rescue robot system is mainly composed of a driving system located at the proximal end, a slender flexible tube with a diameter of 40 mm and a length of 3 m, an automatic insertion system consisting of a crawling mechanism and a pushing/pulling mechanism, and a compliant manipulator mounted at the distal end of the slender tube. A micro-camera integrated with audio intercom system (ASK-40, Mensa Inc.) is installed on the front of the manipulator for the purpose of collecting the environment information and guiding the rescue operation. The rescuer is able to get the view of micro-camera from the monitor and communicate with the survivors through intercom to acquire their real-time conditions and requirements. There are three degrees of freedom (DOFs) adjusting the orientation and posture of compliant manipulator and an extra DOF
Fig. 1. Schematic diagram of the tendon-sheath actuated rescue robot system.
performing grasping operation. The food, water, fresh air, and emergency medicine can be transferred to survivors through the slender flexible tube. In some situations, the manipulator needs to feed the survivors, or even provide simple treatment to the injured victims. The CAD model and geometrical dimensions of the compliant manipulator is presented in Fig. 2. As can be observed, the tendon-sheath driven manipulator can be basically divided into three parts: the gripper, the rotary component and the bending component. The gripper will clamp and grasp the object if the tendon attached to the internal beam of gripper is pulled. On the contrary, because of the recoverable property of elastic deformation, the gripper will automatically open if the tendon is relaxed. The rotary joint plays a significant role during rescue operation. When the tendon fixed on the groove of driving pulley is pulled, the rotary component is able to perform the rolling movement in the range of ± 90°. The bending component consists of two C-shape compliant bending units, which are mutually orthogonal and can perform the yawing movement and the pitching movement in the range of ± 45° respectively. Fig. 3 illustrates the photographs of the compliant manipulator performing different actions [19]. The tendon sheath transmission mechanisms packaged inside the slender flexible tube are capable of providing remote power transmission through the narrow tortuous space. The tendons utilized in the robot system are stainless steel wires with a diameter of 0.8 mm. The sheaths are tightly wound springs made from alloy steel wire and having an inner diameter of 1.2 mm. With the tendon sheath transmission mechanisms, all of the active joints and the gripper of manipulator are actuated by the servo motors placed in the base box of driving system. Therefore, the actuation system can be separated from the end-effector. It is beneficial to simplify the mechanical structure design and achieve low body weight and small dimension. A worm driven crawling mechanism is connected between the compliant manipulator and the slender tube to insert the rescue system into the crevices of ruins. In addition, the pushing/pulling mechanism located on the tail of the tube is actuated via a stepper motor and capable of providing pulling force and pushing force to the tube via the action of friction. If the rescue system is trapped in the ruins, it can be pulled back easily with the cooperation of crawling mechanism and pushing/pulling mechanism. The detailed descriptions of the automatic insertion system have been introduced in our previous researches [20,21]. Each servo motor is equipped with a pulley to its shaft to convert driving toque into pulling force. The driving system is controlled via a microcontroller (C8051F040, Silicon Labs Inc.) which can process data and perform computation internally. All the sub-control systems are connected together to communicate with the host laptop computer (ThinkPad-E450, Lenovo Inc.) via a CAN bus network.
Q. Wu et al. / Mechatronics 46 (2017) 143–153
145
Fig. 2. CAD model of the compliant manipulator. (a) Integral structure; (b) Gripper; (c) Bending component; (d) Rotary component. The gripper is driven by tendon-1 and sheath-1. The bending joint-1 is driven by tendon-2, tendon-3, sheath-2, and sheath-3. The bending joint-2 is driven by tendon-4, tendon-5, sheath-4, and sheath-5. The rotary joint is driven by tendon-6, sheath-6, and sheath-7.
3. Calibration and modeling of the compliant gripper The sensing detection method and the kinematic and dynamic characteristics of the compliant manipulator are far more complicated when compared with those of the traditional rigid robot manipulator. This paper is mainly focused on researching the calibration method, modeling approach, and control strategy of the developed compliant gripper. The compliant rotary component and bending component previously mentioned can be analyzed and studied in similar ways. 3.1. Development and calibration of strain gauge sensors
Fig. 3. Photographs of the compliant manipulator performing different actions. (a) Grasping movement; (b) Yawing movement; (c) Pitching movement; (d) Rolling movement.
For the purpose of measuring the gripper displacement and clamping force without additional nonintegrated sensors, two groups of high-precision resistance strain gauges were attached to the large-strain positions of the gripper [22]. The finite element analysis (FEA) of gripper was performed using SolidWorks/Simulation software package to determine the target locations. The material parameters of compliant gripper, which made of beryllium bronze, are specified as follows: elasticity modulus = 128 GPa, yield strength = 1035 MPa, density = 8260 kg/m3 , and Poisson’s ratio = 0.33. During FEA simulation, a pulling force with a value of F = 10 N was applied to tendon. The base of gripper was set to be immobilized. The FEA result with 68,600 elements is shown in Fig. 4. As can be observed, the induced large-strain positions appear around the inner ends of the gripper, which are marked as point a, point b, point c and point d. The maximum stress is about 240 MPa, which is smaller than the yield strength. Thus, to achieve self-sensing capacity, four strain gauge sensors are symmetrically arranged on these large-strain positions of elastic beams, which are connected into a double-arm bridge to compensate temperature variation and linearize output. The gripper with
146
Q. Wu et al. / Mechatronics 46 (2017) 143–153
Fig. 4. FEA results of the gripper when a pulling force with a value of 10 N is applied to the tendon.
Fig. 5. Schematic diagram of the gripper utilizing strain gauges to measure clamping force and displacement.
strain gauges is shown in Fig. 5. The strain gauges were divided into two groups according to the corresponding detected objects. More specifically, the strain gauge group I is capable of detecting the displacement of the gripper, while the strain gauge group II is capable of detecting the clamping force. Two high precision pressure sensors (FlexiForce A201, Tekscan) were attached to the top and bottom surfaces of the clamped object to obtain the actual clamping force. Besides, a linear position sensor (KTC-1B, MIRAN) was connected to one of elastic beam of gripper. It should be noted that, the force required to drive the pushrod of the linear position sensor is less than 0.05 N and, thus, can be neglected during practical operation. Calibration experiments were conducted to establish internal relations between the output signals from strain gauges and the displacement and clamping force of gripper. The voltages of strain gauges I and strain gauges II were compared with the outputs of high-precision position sensor and pressure sensor. The experimental setup of the compliant gripper control system was developed in the MATLAB/Simulink (Mathworks Inc.) environment with the Real-Time-Workshop (RTW) kernel, as depicted in Fig. 6. The platform mainly consists of a servo AC motor (SGDV-2R8A0, YASKAWA Inc.) as the actuator, two industrial personal computers (IPC-610H, Advantech Inc.) as the host computer and the real-time target, a tendon-sheath transmission system utilized to delivered the pulling force to the gripper with four strain gauge sensors, and two high precision pressure sensors (FlexiForce A201, Tekscan) and a linear position sensor (KTC-1B, MIRAN) to measure the clamping force and displacement of gripper. The tendon-sheath transmission system includes a flexible outer sheath and an inner tendon. The proximal and distal ends of tendon were attached to a pulley mounted on the motor shaft, while the distal end of tendon was attached to the gripper. Then the pulling force from the motor can be transmitted to the gripper by displacement between the tendon and sheath. The inner tendon of the platform is stainless steel wire with a diameter of 0.8 mm and a length of 0.8 m.
Fig. 6. Experimental setup of the compliant gripper control system. (a) Schematic diagram; (b) Photograph of the experimental configuration.
The outer sheath has an inner diameter of 1.2 mm and a length of 0.6 m. The analog sensor signals were first amplified by a transistor made voltage amplifier, and then acquired and processed by an A/D data acquisition card (PCL-818HD, Advantech Inc.). A fifthorder low-pass Bessel filter with a cut-off frequency of 35 rad/s was made use to filter the acquired analog signals. The digital motion control command, generated in the real-time target, were transformed into analog output signal via a D/A card (PCL-726, Advantech Inc.) and transmitted to the motor servo driver with a sampling rate of 10 kHz. In the calibration experiments, the calibration ranges of the strain-gauge displacement sensor and force sensor are about [0 mm, 11 mm] and [0 N, 19 N] with 20 sampling points, respectively. The calibration results are shown in Figs. 7(a) and 8(a), where the actual gripper displacement and clamping force (vertical axis) are plotted against the voltages of strain gauges (horizontal axis). It can be clearly observed that the experimental results show approximately linear relationship between the strain signals and the corresponding measuring parameters. The least square method was applied to conduct the multipoint linear fitting process. As a result, the following relationships hold:
x = 8.23VI − 0.22 mm
(1)
Fc = 12.31VII − 10.59 N
(2)
Q. Wu et al. / Mechatronics 46 (2017) 143–153
147
Fig. 7. Calibration results of the strain-gauge displacement sensor. (a) The relationship between the voltage of strain gauges I and the gripper displacement; (b) Time history and (c) histogram of the noise of strain gauges I.
Fig. 8. Calibration results of the strain-gauge force sensor. (a) The relationship between the voltage of strain gauges II and the clamping force; (b) Time history and (c) histogram of the noise of strain gauges II.
where x and Fc denote the displacement and clamping force of gripper; VI and VII represent the detected output voltages of strain gauges I and strain gauges II. The measurement white noises of strain gauges with zero voltage inputs within five seconds were acquired and depicted in Figs. 7(b) and 8(b). Meanwhile, the histograms of the measurement noises were shown in Fig. 7(c) and 8(c). We can see that the measurement white noises approximately followed normal distribution. More specifically, the mean and standard deviation of the white noises of strain gauges I were 0.0049 mm and 0.0946 mm, which reveals that 95% of the noise values were located in the interval of 0.0049 ± 0.1854 mm around the mean value. On the other hand, the 95% confidence interval of the white noises of strain gauges II was 0.0305 ± 0.3483 N. 3.2. System modeling and parameters identification According to the existing literatures [23–24], the compliant manipulating gripper system (including the compliant gripper and the tendon-sheath transmission system) can be simplified into a massspring-damper system with a dynamic model shown as follows:
Mx¨ (t ) + Bx˙ (t ) + Kx(t ) + P (t ) = τ (t )
(3)
where t denotes the time variable; x denotes the displacement of gripper; M, B, and K represent the parameters of mass, damping, and stiffness of gripper respectively; P(t) describes the lumped effects of system friction, tendon-sheath elongation and hysteresis, and external disturbance; τ (t) is output torque of driving motor. Open-loop frequency response experiments were conducted to identify the dynamics parameters of the compliant gripper system
with tendon sheath actuation. The dynamic model reveals the relationship between the output driving torque of servo motor and the displacement, velocity and acceleration of gripper. Tendon-sheath actuation system is capable of providing remote power transmission. However, due to the tendon compliance and the friction between tendon and sheath, there exist many undesirable nonlinear problems, such as backlash, dead zone, hysteresis, slacking and direction-dependent behavior. Moreover, the transmission property is affected by the total bending angle of system, which may vary during the rescue operation [25]. In our previous research [26], the transmission characteristic and model identification approach of the tendon-sheath actuation system have been described and analyzed in detail. This section is focused on analyzing the dynamic model of compliant gripper. In order to minimize the effects from tendon-sheath system, the total tendon bending angle of the experimental setup was set to zero during the identification experiments, as shown in Fig. 6(b). Therefore, the effect of the lumped uncertainties and tendon-sheath system, i.e., P(t), can be neglected during parameters identification. The servo motor was set to run in torque control mode and required to provide sinusoidal driving torque with constant amplitude of 0.5 Nm and various frequencies ranging from 0.1 to 40 Hz. Fig. 9 illustrates the Bode diagram of open-loop frequency response of the compliant gripper, which describes the transfer from the input driving torque to the output gripper displacement. From the magnitude and phase characteristics, it can be observed that dynamic model of gripper can be approximated as a second order linear model if the driving frequency is lower than 10 Hz. More specifically, the system contains a resonant peak around 6 Hz with a magnitude value of 28.33 dB, and
148
Q. Wu et al. / Mechatronics 46 (2017) 143–153
Fig. 10. Schematic diagram of the Cascaded PID controller for clamping force.
Fig. 9. Bode diagram of frequency response of compliant gripper. (a) Magnitude characteristic; (b) Phase characteristic.
the magnitudes in the frequencies below 1 Hz are about 25 dB. According to the experimental data at frequencies lower than 10 Hz, the second order model can be well fit by using the discrete data fitting tool of MATLAB (MathWorks, Inc.). The identified dynamic model can be described by
G (s ) = =
x (s ) 1 = τ (s ) Mˆ s2 + Bˆs + Kˆ 18.94
4.091 ×
10−4 s2
+ 1.576 × 10−2 s + 1
(4)
ˆ , Bˆ, and Kˆ represents the estimated parameters of mass, where M damping coefficient, and stiffness of gripper respectively. The experimental results and fitting results are compared in Fig. 9. It can be seen that the fitting results of the second order model matches the experimental results well in the frequencies lower than 10 Hz. For the frequency higher than 10 Hz, a higher order model is required to describe the dynamic model of gripper. However, since the necessary bandwidth of rescue operation is lower than 10 Hz, the developed simplified dynamic model can be applied in the control strategy of practical application.
was made use to restrict the maximum driving force and guarantee operation safety. The proportional gain Kp , integral gain Ki , and derivative gain Kd of the outer loop PID controller and inner loop PID controller were estimated via Ziegler-Nichols method [27] and optimally tuned by intensive tests to improve control accuracy and stability. Force tracking experiments were carried out using the real-time experimental setup (Fig. 6). Experimental performance of the developed cascade PID controller was compared to that of a PID controller. The gripper was required to grasp the target object and control the clamping force to follow a sinusoid wave signal with time-varying frequency and peak-to-peak amplitude. The first and last four seconds of the sinusoid signal saw a frequency of 0.25 Hz and an amplitude of 20 N. From the moment t = 4 s to t = 7 s and t = 11 s to t = 14 s, the frequency and amplitude changed to 0.33 Hz and 16 N. From the moment t = 7 s to t = 11 s, the frequency and amplitude turned to 0.5 Hz and 12 N, respectively. The selected control parameters of the cascade PID controller and PID controller are presented in Table 1. The results of force tracking experiments with different control strategies are compared and depicted in Fig. 11(a) and (b). As can be observed, the tracking performance of the cascade PID controller goes much better than that of the PID controller. The root mean square errors (RMSE) decline from 2.016 N (PID) to 0.821 N (cascade PID). Regarding the peak to peak errors, the cascade PID algorithm gives very small value, i.e., 1.586 N, in comparison with the value of PID controller, i.e., 3.975 N. The histogram of the force tracking errors with cascade PID controller is plotted in Fig. 11(c). The 95% confidence interval of the force tracking errors was −0.0129 ± 1.6091 N. Therefore, the results of force tracking experiment demonstrate the efficacies of the strain-gauge force sensor and the cascade PID controller. 4.2. Position control strategy
4. Control methods and experimental validation 4.1. Force control strategy The control of the clamping force of gripper is realized by utilizing a cascade PID controller, which is capable of responding to the external disturbance rapidly and improving trajectory tracking performance. The schematic diagram of the controller is shown in Fig. 10. The analog output signals from strain gauges II are firstly filtrated via a Bessel low pass analog filter with a cut-off frequency of 35 rad/s, and then converted into the clamping force value F according to the relationship shown in Eq. (2), and finally compared with the desired force Fd . The outer loop PID regulator operates the force error between Fd and F, and the output is added with the comparison error of the time rate of changes of desired force and measured force and, after that, sent to the inner PID regulator. Finally, the inner PID regulator outputs a desired voltage with a saturation interval of ± 5 V to servo controller. The servo motor was set to run in torque control mode, and the saturation module
In this section, a FSMC law is proposed to regulate the displacement of gripper. The control objective is to develop an applicable robust control strategy with favorable performance, so that the gripper can follow expected reference position trajectory under undesirable influence of bounded uncertainties. First, Eq. (3) can be refined as:
τ (t ) − MB x˙ (t ) − MK x(t ) − MP (t ) = A p τ (t ) + B p x˙ (t ) + C p x(t ) + D p (t )
x¨ (t ) =
1 M
(5)
where Ap = 1/M, Bp = −B/M, Cp = −K/M, and Dp = −P/M. Consider Eq. (5) with unpredicted model parameter uncertainties for actual gripper, the system dynamic model can be rewritten as:
x¨ (t ) = (A p + A )τ (t ) + (B p + B )x˙ (t ) + (C p + C )x(t ) + D p (t ) = A p τ (t ) + B p x˙ (t ) + C p x(t ) + L(t ) (6)
Q. Wu et al. / Mechatronics 46 (2017) 143–153
149
Table 1 The parameters of cascade PID controller. Parameters
Kp Ki Kd
Cascaded PID controller
PID controller
Outer loop PID controller
Inner loop PID controller
1.2 0.8 0.2
3.6 2.0 0.5
4.1 2.5 1.0
Fig. 11. Results of force tracking experiment. (a) Comparisons of desired trajectory and experimental trajectory; (b) Time history and (c) histogram of the force tracking errors.
L(t ) = Aτ (t ) + Bx˙ (t ) + Cx(t ) + D p (t )
(7)
where A, B, and C represent the uncertainties of system parameters; L(t) denotes the lumped uncertainty, whose boundary is assumed to be given as:
|L(t )| < η
(8)
where η represents a predetermined positive constant. A sliding model controller with a PD sliding surface is developed to enforce the system state trajectory to approach to the sliding surface and ensure expected closed-loop control performance and specifications. The position, velocity and acceleration tracking errors are defined as:
e(t ) = xd (t ) − x(t )
(9)
e˙ (t ) = x˙ d (t ) − x˙ (t )
(10)
e¨ (t ) = x¨d (t ) − x¨ (t )
(11)
where xd (t), x˙ d (t ) and x¨d (t ) are the desired position, velocity and acceleration. The switching function of the sliding surface is defined in the state-space R2 and expressed as the following equation:
S(t ) = e˙ (t ) + λe(t )
(12)
where λ is a positive constant of proportional gain. The expression for the deviation of the sliding variable can be shown as:
S˙ (t ) = e¨ (t ) + λe˙ (t )
(13)
Then, inserting Eqs. (6) and (11) into (13), the following equation can be obtained:
S˙ (t ) = x¨d (t ) + λe˙ (t ) − A p τ (t ) − B p x˙ (t ) − C p x(t ) − L(t )
(14)
In the conventional design of SMC system, the control law U(t) is basically composed of two terms, i.e. the equivalent control term
Ueq (t) and the hitting control term Uhit (t), and can be represented in the general form as follows [28]:
U (t ) = Ueq (t ) + Uhit (t )
(15)
The equivalent control term Ueq (t), which determines the dynamic performance of system on the sliding surface, can be derived as the solution of S˙ (t ) = 0 without considering any lumped uncertainty, i.e. L(t) = 0. Thus, from Eq. (14), we can get:
Ueq (t ) =
Bp Cp 1 λ e˙ (t ) − x˙ (t ) − x(t ) x¨ (t ) + Ap d Ap Ap Ap
(16)
However, the equivalent control effort alone cannot ensure the control performance if the initial system state does not lie on the sliding surface or parameters variations and undesirable disturbance occur during motion. Therefore, the hitting control term Uhit (t) should be applied to eliminate the effect of perturbations and drive the system toward the reference trajectory. As it is well known, the sign function can be utilized to establish the hitting control law and ensure the globally stable equilibrium of system. However, it may lead to heavily chattering phenomenon and high-frequency unmodeled dynamics. Thus, in order to mitigate the undesirable chattering problem, the hitting control law is realized by using a fuzzy logic inference mechanism in our research. The overall schematic diagram of the FSMC is illustrated in Fig. 12. The analog output signals from strain gauges I were filtrated via a Bessel low pass analog filter with a cut-off frequency of 35 rad/s, and converted into the gripper displacement x according to Eq. (1). The hitting control law can be represented as:
Uhit (t ) = FC(S(t ) )
(17)
where FC(·) denotes the proposed nonlinear fuzzy controller with the sliding variable S(t) and the hitting control term Uhit (t) as the input linguistic variable and output linguistic variable, respectively. The fuzzy logic controller is developed by utilizing the Mamdani inference method [29] and applied to the SMC algorithm for online turning of Uhit (t). In the fuzzification process, the input of fuzzy logic is scaled and mapped into three trigonometry/trapezoidal shape membership functions, namely, negative (N),
150
Q. Wu et al. / Mechatronics 46 (2017) 143–153
Here 0 ≤ A(rj ) ≤ 1 represents the firing strength of rule j. Moreover, the sum of all firing strengths is equal to one, i.e. A(r1 ) + A(r2 ) + A(r3 ) = 1. r1 = −r, r2 = 0 and r3 = r denote the center of the membership functions NE, ZE, and PE, respectively. r is a positive constant. And then, the value of hitting control term can be analyzed according to the value of sliding variable S(t). According to Fig. 13, the following four situations shown in Table 3 can be obtained: From the above analyzing results of different situations, it can be observed that 0 ≤ S(t)[A(r3 ) − A(r1 )] = |S(t)|·|A(r3 ) − A(r1 )|. Furthermore, the entire control law shown in Eq. (15) can be represented as:
U (t ) = Ueq (t ) + r [A(r3 ) − A(r1 )]
Fig. 12. Schematic diagram of the FSMC strategy for gripper displacement.
(19)
For the purpose of demonstrating the system stability with the proposed FSMC scheme, the following Lyapunov candidate function is proposed [30]:
V (t ) =
S2 (t ) 2
(20)
Differentiating S(t) with respect to time and utilizing Eqs. (14)– (16) and (19) gives:
V˙ (t ) = S(t )S˙ (t ) = S(t )[x¨d (t ) + λe˙ (t ) − A pUeq (t ) − A pUhit (t ) − B p x˙ (t ) − C p x(t ) − L(t )] = −S(t )A pUhit (t ) − S(t )L(t ) Fig. 13. The input membership function for S(t). N: negative; Z: zero; P: positive.
≤ −A p r |S(t )||A(r3 ) − A(r1 )| + |S(t )||L(t )| = −|S(t )|[r A p |A(r3 ) − A(r1 )| − |L(t )|]
Fig. 14. The output membership function for Uhit (t). NE: negative effort; ZE: zero effort; PE: positive effort. Table 2 The fuzzy control rules for Uhit (t). Rule
S(t)
Uhit (t)
1 2 3
N Z P
NE ZE PE
zero (Z) and positive (P) respectively, as shown in Fig. 13. The control actions of fuzzy logic are divided into three fuzzy sets, namely, negative effort (NE), zero effort (ZE) and positive effort (PE) respectively, as shown in Fig. 14. Each combination of scaled input and membership function combination activates one control action according to the inference rules. The fuzzy linguistic rules are defined in Table 2. The center-of-gravity method is chosen as the defuzzification strategy to compute the output variable of fuzzy logic. It guarantees high control sensitivity to the changes of input signals and, meanwhile, avoids time-consuming calculation process. The output value can be calculated as follows:
3 j=1
Uhit (t ) = 3
r j A (r j )
j=1
A (r j )
= r1 A ( r1 ) + r2 A ( r2 ) + r3 A ( r3 )
(18)
(21)
If we select r > |L(t )|/[A p |A(r3 ) − A(r1 )|], the reaching condition V˙ (t ) = S(t )S˙ (t ) < 0 is always satisfied. It should be noted that V(t)is positive definite and V˙ (t ) is negative definite. Besides, when S(t) tends to infinity, V(t)also approaches to infinity. Consequently, the closed loop control system is globally asymptotically stable. The position tracking error state gradually converges to the sliding surface S(t) = 0 as t → ∞. Position tracking experiment was conducted in the experimental setup shown in Fig. 6. Experimental performance of the developed FSMC algorithm was compared to that of a conventional SMC algorithm [31]. The desired gripper displacement was defined to follow a sinusoid wave trajectory with time-varying frequency and peak-to-peak amplitude. The first eight seconds of the sinusoid trajectory had a frequency of 0.25 Hz and an amplitude of 10 mm, which then changed to 0.33 Hz and 12 mm from the moment t = 8 s to t = 14 s. In the last six seconds, the frequency and amplitude changed to 0.5 Hz and 8 mm, respectively. It needs to be pointed out that, during the tracking experiment, the feedback signals applied in the position controller came from the strain-gauge sensors, while the actual gripper displacement was detected via the highprecision position sensor. The parameters of the proposed FSMC and conventional SMC algorithms were empirically adjusted to achieve superior control performance. During the experiments, the parameters of FSMC were set to λ = 6.8, S1 = −1.2, S2 = 1.2, r = 3.5. The dynamic model parameters of gripper have been identified in the previous research. The results of position tracking experiments with different control strategies are compared in Fig. 15(a) and (b). It can be observed that the tracking performance of FSMC has turned out to be far more desirable. More specifically, the root mean square errors (RMSE) decline from 1.216 mm (SMC) to 0.383 mm (FSMC). The peak to peak error of the FSMC algorithm (i.e., 1.005 mm) is smaller than the value of SMC algorithm (i.e., 2.694 mm). Besides, according to Fig. 15(b), the chattering level can be significantly reduced with FSMC algorithm. The histogram of the position tracking errors under FSMC is presented in Fig. 15(c).
Q. Wu et al. / Mechatronics 46 (2017) 143–153
151
Table 3 The values of hitting control term in different situations. Condition
Activated rules
A(r1 )
A(r2 )
A(r3 )
Uhit (t)
S2 < S(t) 0 < S(t) ≤ S2 S1 < S(t) ≤ 0 S1 > S(t)
Rule Rule Rule Rule
A(r1 )= 0 A(r1 )= 0 0 < A(r1 ) ≤ 1 A(r1 )= 1
A(r2 )= 0 0 < A(r2 ) ≤ 1 0 < A(r2 ) ≤ 1 A(r2 )= 0
A(r3 )= 1 0 < A(r3 ) ≤ 1 A(r3 ) = 0 A(r3 )= 0
Uhit (t) = r3 Uhit (t) = r2 Uhit (t) = r1 Uhit (t) = r1
3 2 and rule 3 1 and rule 2 1
A(r3 ) = r A(r2 ) + r3 A(r3 ) = r A(r3 ) A(r1 ) + r2 A(r2 ) = −r A(r1 ) A(r1 ) = −r
Fig. 15. Results of position tracking experiment. (a) Comparisons of desired trajectory and experimental trajectory; (b) Time history and (c) histogram of the position tracking errors.
The tracking errors complied with the normal distribution approximately and the 95% confidence interval was 0.0059 ± 0.7524 mm. The results of position tracking experiment verify the accuracy of the developed strain-gauge position sensor and the effectiveness of FSMC strategy. 4.3. Hybrid force/position switching control strategy In the aforementioned sections, the cascaded PID controller and FSMC law have been developed to guarantee the effectiveness of the force tracking control and position tracking control of gripper. This section deals with a hybrid force/position switching control strategy utilized to coordinate the relationship between force control and position control during operation. In order to perform a completed manipulation task, the handling procedure of the compliant gripper falls mostly into three steps: closing step, clamping step, and opening step. Initially, there is no pulling force acting on the inner tendon, and the two elastic beams of gripper maintain in their home positions. In the duration of closing step, the gripper is driven to approach to the object and execute the grasping action. It comes to the clamping step after the target object contacts with the gripper and is removed to a desired location. In this phase, the clamping force varies with the actuation of inner tendon, and the gripper displacement stabilizes on the maximum value. The opening step occurs after the object is loosened and placed on the destination. In this phase, the clamping force decreases to zero, while the gripper displacement is reduced under the tendon-sheath actuation. The analysis of operational process indicates that the position control performance is crucial to the closing step and opening step, while the accurate force control strategy should be executed during clamping procedure. Therefore, a hybrid force/position switching control strategy, which is integrated with the previously developed cascaded PID force controller and fuzzy sliding mode position controller, is proposed to ensure smooth transition between different working steps and achieve satisfied control results, as illustrated in Fig. 16. The switching criteria are established based on the detected clamping force. More specifically, the hybrid controller will be switched to force control mode if the gripper contacts with the object and the clamp-
Fig. 16. Schematic diagram of the hybrid force/position switching control strategy.
ing force is larger than the predefined threshold value Fs . Otherwise, if the clamping force is equal to or smaller than the threshold value Fs , the hybrid controller will be switched to the position control mode to make the gripper track the reference position trajectory accurately. The threshold value is determined according to the physical characteristics of the grasped object and the requirements of practical application. Experimental investigations were conducted to validate the effectiveness of the hybrid force/position switching control scheme and evaluate the performance of the grasping process. Firstly, the elastic beams of gripper were expected to move towards the target object with a constant velocity of 2 mm/s during the closing step. And then, after the gripper contacts with the object, the clamping force was required to maintain at the value of 8 N during the clamping step. The threshold value Fs of the switching criteria was set as 0.5 N. Finally, during the opening step, the desired clamping force was reduced to 0 N, while the elastic beams were expected to move away from the object and return to the initial positions in 5 s. In addition, the reference position trajectory of returning movement was defined as a cubic polynomial with continuous velocity. The predetermined trajectories of gripper displacement and clamping force and the experimental results are shown in Fig. 17(a) and 17(c). As can be observed, the first 4.8 s saw the closing of gripper, and the maximum displacement is about 9.6 mm. From t = 4.8 s to t = 10 s, the value of clamping force was regulated by
152
Q. Wu et al. / Mechatronics 46 (2017) 143–153
the force controller and kept around 8 N. The last 5 s saw the opening of gripper under the regulation of position controller. The position and force tracking errors are presented in Fig. 17(b) and (d) respectively. The RMSE of position and force tracking errors were calculated to be 0.268 mm and 0.385 N, which were about 3.68% and 8.34% of the RMS values of the desired position and force trajectories. Furthermore, the relationship between gripper displacement and clamping force are shown in Fig. 17(e). It is found that there was no clamping force during the closing and opening processes. The gripper displacement was kept in its maximum value while the object was grasped with a time-variable clamping force, mainly due to the high structural stiffness of the object and compliant property of the developed gripper. The experimental results demonstrate that the developed hybrid force/position control law is capable of regulating the gripper position and clamping force and achieving smooth interaction. 5. Conclusion and future works This paper dealt with the development and research of a long and slender rescue robot with compliant manipulator, which can work in the narrow and confine space of ruins for disaster research and rescue. Firstly, the major mechanical structure and control system of the tendon-sheath actuated rescue robot were introduced. And then, the research was mainly focused on the analysis and control of the compliant gripper. With the aim of measuring the gripper displacement and clamping force, two groups of resistive strain gages were attached to the large-strain positions of gripper under external tendon pulling force according to FEA simulation results. The sensors calibration and model identification were carried out based on the experimental setups established in Matlab/ RTW environment. In order to improve the operation performance of the gripper, a cascaded PID controller and a FSMC algorithm were developed to ensure the force and position tracking accuracies. In addition, a hybrid force/position control scheme was presented to coordinate the relationship between force control and position control while executing the object grasping task. Finally, the effectiveness of the developed force/position sensing approach and control algorithms were validated by trajectories tracking experiments with time-varying frequency and amplitude and closingclamping-opening operation of gripper. Future works will be devoted to further improve the proposed force/position control algorithm with the aim of efficiently reducing the tracking errors and achieving more complicated manipulation tasks. The dynamic calibration will be conducted to improve the measure accuracy of selfsensing system. Besides, the modeling and control of the entire compliant manipulator will be researched and applied to rescue application. These works are currently underway. Acknowledgments This research has been supported by the Natural Science Foundation of Jiangsu Province of China (BK20170783), the China Nation Nature Science Foundation (51575100 and 51575256), the NUAA Fundamental Research Funds (NS2013042), the NUAA Scientific Research Funds (90YAH16085). References Fig. 17. Results of the hybrid force/position tracking control experiments. (a) Gripper displacement tracking result; (b) Displacement tracking error; (c) Clamping force tracking result; (d) Force tracking error; (e) Relationship between gripper displacement and clamping force.
[1] Jang H C, Lien Y N, Tsai T C. Rescue information system for earthquake disasters based on MANET emergency communication platform. In: Proceedings of the 2009 International Conference on Wireless Communications and Mobile Computing: Connecting the World Wirelessly; 2009: 623–627. [2] Murphy R, Tadikoro S, Nardi D, et al. Search and Rescue Robotics. In: Springer handbook of robotics, part F: field and service robotics; 2007. p. 1151–73. [3] Li J, Wang X, Xi R. Tip force tracking control based on the tendon-sheath transmission model and 3D reconstruction of a long slender rescue robot. In: IEEE 14th international workshop on advanced motion control; 2016. p. 448–54.
Q. Wu et al. / Mechatronics 46 (2017) 143–153 [4] Arai M, Takayama T, Hirose S. Development of “Souryu-III” connected crawler vehicle for inspection inside narrow and winding spaces. In: IEEE/RSJ int. conf. intell. robots and sys; 2004. p. 52–7. [5] Arai M, Yoshinori T, Hirose S, Kuwahara H, Tsukui C. Development of ‘Souryu-IV’ and ‘Souryu-V:’ serially connected crawler vehicles for in-rubble searching operations. J Field Robot 2008;25:31–65. [6] Kitagawa A, Tsukagoshi H, Igarashi M. Development of small diameter active hose-ii for search and life-prolongation of victimsunder debris. J Robot Mech 2003;15:474–81. [7] Kazuyukilto HA. Development of rescue manipulator to search narrow space for victims. Artif Life Robotics 2008;13:331–5. [8] Chen L, Wang X. Design and preliminary experiment of a robot for searching interior rubble. In: international conference on systems, man, and cybernetics (SMC); 2011. p. 1956–61. [9] Liu L, Leonhardt S, Misgeld BJE. Design and control of a mechanical rotary variable impedance actuator. Mechatron 2016;39:226–36. [10] Campeau-Lecours A, Otis M, Belzile PL, Gosselin C. A time-domain vibration observer and controller for physical human-robot interaction. Mechatron 2016;36:45–53. [11] Lin J, Lin CC. Hybrid fuzzy position/force control by adaptive network-based fuzzy inference system for robot manipulator mounted on oscillatory base. J Vib Control 2015;21(10):1930–45. [12] Calanca A, Muradore R, Fiorini P. A review of algorithms for compliant control of stiff and fixed-compliance robots. IEEE-ASME T Mech 2016;21(2):613–24. [13] Kim HS, Kim IM, Cho CN, et al. Safe joint module for safe robot arm based on passive and active compliance method. Mechatron 2012;22(7):1023–30. [14] Khan SG, Herrmann G, Lenz A, Mubarak A, Pipe T, Melhuish C. Compliance control and human–robot interaction: part II—experimental examples. Int J Hum Robot 2014;11(03):1430 0 02. [15] Denkena B, Kiesner J. Strain gauge based sensing hydraulic fixtures. Mechatron 2016;34:111–18. [16] Yang Y, Wei Y, Lou J, et al. Development and precision position/force control of a new flexure-based microgripper. J Micromech Microeng 2015;26(1):015005. [17] Xu Q. Design of a large-range compliant rotary micropositioning stage with angle and torque sensing. IEEE Sens J 2015;15(4):2419–30. [18] Leang KK, Shan Y, Song S, et al. Integrated sensing for IPMC actuators using strain gages for underwater applications. IEEE-ASME T Mech 2012;17(2):345–55. [19] Kai W, Xingsong W. Preliminary experiments on soft manipulating of tendon-sheath-driven compliant joints. In: 2nd international conference on robot intelligence technology and applications; 2014. p. 503–15. [20] Zhang G, Wang X. Design and 3D reconstruction of a Tendon-Sheath-Driven searching and rescuing robot. In: IEEE international symposium on safety, security, and rescue robotics (SSRR); 2013. p. 1–6. [21] Zhang G, Wang X. Design of automatic insertion system for a tendon-sheath– driven searching and rescuing robot. In: 3rd international conference on mechatronics and applied mechanics; 2013. p. 175–8. [22] Leang KK, Shan Y, Song S, Kwang J. Integrated sensing for IPMC actuators using strain gages for underwater applications. IEEE-ASME T Mech 2012;17(2):345–55. [23] Xu Q, Li Y. Model predictive discrete-time sliding mode control of a nanopositioning piezostage without modeling hysteresis. IEEE T Contr Syst T 2012;20(4):983–94. [24] Xu Q. Design, testing and precision control of a novel long-stroke flexure micropositioning system. Mech Mach Theory 2013;70:209–24. [25] Chen L, Wang X, Xu WL. Inverse transmission model and compensation control of a single-tendon–sheath actuator. IEEE T Ind Electron 2014;61(3):1424–33. [26] Wu Q, Wang X, Chen L, Du F. Transmission model and compensation control of double-tendon-sheath actuation system. IEEE T Ind Electron 2015;62(3):1599–609. [27] Ziegler JG, Nichols NB. Optimum settings for automatic controllers. Trans. ASME 1942;64(11). [28] Wu Q, Wang X, Chen L, Du F, Xi R. Modeling and position control of a therapeutic exoskeleton targeting upper extremity rehabilitation. P I Mech Eng C-J Mec 2016; 0954406216668204. [29] Nagi F, Perumal L, Nagi J. A new integrated fuzzy bang–bang relay control system. Mechatron 2009;19(5):748–60. [30] Chen C, Ren CE, Du T. Fuzzy observed-based adaptive consensus tracking control for second-order multi-agent systems with heterogeneous nonlinear dynamics. IEEE T Fuzzy Syst 2015;24(4):906–15. [31] Young KK. Controller design for a manipulator using theory of variable structure systems. IEEE Trans Syst Man Cyber 1978;8(2):101–9.
153 Qingcong Wu received the B.S. and Ph.D. degrees in mechatronics engineering from Southeast University, Nanjing, China, in 2011 and 2016, respectively. He is currently a Lecturer with the College of Mechanical and Electrical Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing, China. His major research interests include robotics, tendon-sheath transmission theory, gravity balancing theory, and the application of exoskeleton to neuromuscular rehabilitation.
Xingsong Wang received the B.S. degree and M.S. degree from Zhejiang University, Hangzhou, China, in 1988 and 1991, respectively, and the Ph.D. degree from Southeast University, Nanjing, China, in 20 0 0, all in mechanical engineering. He was a visiting scientist at Concordia University, Canada (20 0 0.6–20 0 0.12, 20 01.9–20 02.3) and at Purdue University, USA (20 07.9–20 08.3), both at School of Mechanical Engineering. Currently, he is a full professor in the School of Mechanical Engineering and head of Department of Mechatronics at Southeast University. His current research interests include control theory with application in precision CNC machine tools, advanced mechatronics with applications in biomedical engineering, tendon-sheath transmission theory with application in rescue robots, and Mecanum-wheels based AGV systems designing. Bai Chen received the B.S. degree and Ph.D. degree from Zhejiang University, Hangzhou, China, in 20 0 0 and 20 05, respectively, all in mechanical engineering. Currently, he is a full professor in the College of Mechanical and Electrical Engineering at Nanjing University of Aeronautics and Astronautics. His current research interests include minimally invasive neurosurgery robot, virtual surgery system, force feedback control, interventional therapy.
Hongtao Wu received the B.S. degree from Yanshan University, Hebei, China, in 1982, and the M.S. degree and Ph.D. degree from Tianjin University, Tianjin, China, in 1985 and 1992, respectively, all in mechanical engineering. Currently, he is a full professor in the College of Mechanical and Electrical Engineering at Nanjing University of Aeronautics and Astronautics. His current research interests include parallel robot, robot kinematics, multibody system dynamics.
Ziyan Shao received the B.S. degree and M.S. degree from Yangzhou University, Yangzhou, China, in 2013 and 2016, respectively. Currently he is working toward the Ph.D. degree in the College of Mechanical and Electrical Engineering at Nanjing University of Aeronautics and Astronautics. His major research interests include exoskeleton, robot control, and robot dynamics.
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
Development and hybrid force/position control of a compliant rescue manipulatorR Qingcong Wu a,∗, Xingsong Wang b, Bai Chen a, Hongtao Wu a, Ziyan Shao a a b
College of Mechanical and Electrical Engineering, Nanjing University of Aeronautics and Astronautics, Jiangsu Province 210016, China College of Mechanical Engineering, Southeast University, Jiangsu Province 211100, China
a r t i c l e
i n f o
Article history: Received 23 December 2016 Revised 13 May 2017 Accepted 3 August 2017
Keywords: Rescue robot Compliant gripper Self-sensing calibration Model identification Hybrid force/position control
a b s t r a c t Performing search and rescue tasks in the ruins after disasters demand rescue robots with slender and compliant structure to accommodate the complicated configurations under debris. This paper presents the structural design and system composition of a novel tendon-sheath actuated compliant rescue manipulator with slender and flexible body. The proposed robot can drill into the narrow space where rescuers and traditional rigid robots cannot get in because of size limitation or toxic environment. The self-sensing calibration, dynamic modeling, and hybrid force/position control trajectory of the compliant gripper with integrated position and force monitoring capabilities are analyzed and discussed. With the aim of regulating the gripper displacement and clamping force during operation, a hybrid force/position control strategy is proposed based on a cascaded proportional-integral-derivative (PID) controller and a fuzzy sliding mode controller (FSMC). Experimental setups mainly consisting of servo motor, tendon sheath transmission components, compliant gripper, and real-time control system are established to calibrate the strain gauge sensors and identify the dynamic model parameters. Further experimental investigations involving force tracking experiments, position tracking experiments, and object grasping experiments are carried out. The experimental results demonstrate the effectiveness of the developed self-sensing approach and control strategies during rescue operation. © 2017 Elsevier Ltd. All rights reserved.
1. Introduction In the last decade, large numbers of people have suffered from different kinds of natural or man-made disasters causing large scale of damages, such as earthquake, landslide, mining accidents, floods, and so on. After these disasters, it is of great significant to search the survivors in the ruins and provide necessary medical assistance and treatment as soon as possible. Existing researches indicate that the difficulty and risk of disaster rescue will increase rapidly after the so-called golden seventy-two h [1]. The surface survivors can be easily found and rescued in time. However, for the interior victims trapped inside the confined spaces under rubble piles, it is difficult and dangerous for the responders and canine teams to execute the rescue and relief works due to the harsh conditions. Therefore, more and more researchers have focused their efforts on developing rescue robots to explore deep under debris [2,3].
R ∗
This paper was recommended for publication by Associate Editor Roger Dixon. Corresponding author. E-mail address: [email protected] (Q. Wu).
http://dx.doi.org/10.1016/j.mechatronics.2017.08.003 0957-4158/© 2017 Elsevier Ltd. All rights reserved.
Most of the existing rescue robots were designed with smart size, multiple joints, and strong crawling capacity to drill into small crevices and access victims. Arai et al. [4,5] developed a series of snake-like crawler vehicles called Souryu for in-rubble searching operation. These robots were equipped with motors, cameras, sensors, battery and control unit inside the body and remote-operated via wireless joystick. The large cross section area definitely limited its mobility within narrow space. Kitagawa et al. [6] investigated a small-size rescue robot, named Active Hose, for searching the survivors within collapses. The Active Hose, made up of eccentric wire mechanism, was driven by several wound tube actuators and capable of curving to arbitrary direction. The major drawback of this rescue system is the large friction between robot body and external environment. A rescue manipulator with duplex parallel mechanism was designed by Hirayama and Kazuyukilto [7] in Hosei University to search narrow space for victims. This robot needs to be operated manually by rescuer with the consideration that electrical power supply might not be obtained in practical disaster situation. Chen and Wang [8] have designed a researching robot, composed of modular rigid steering head and flexible body, to work within small voids of rubble and confined space. Preliminary experimental results showed that the rigidity
144
Q. Wu et al. / Mechatronics 46 (2017) 143–153
property of steering head reduced the effectiveness in searching the interior through a rubble channel. Because of space limitations, traditional rescue robots with rigid body have difficulty in adapting to the complicated configurations under debris and executing rescue actions. Besides, in certain cases the end-effector of rescue robot needs to provide necessary assistance to the weaken victims. The compliance control technology, such as impedance control [9], admittance control [10] and hybrid force/position control [11], can be applied to ensure human-robotenvironment interactive safety during rescue operation [12]. Generally, the stiff joints of rigid manipulator need to be equipped with additional nonintegrated force/torque sensors and position sensors, such as capacitance sensors, inductance sensors, laser sensors, and optical reflective sensors, to acquire required feedback information and achieve desired compliance [13,14]. It leads to larger robot dimension and heavier weight and, thus, is not suitable to the miniature robot applications. Integrating the resistive strain gauge sensors with robotic system is an effective measuring method to achieve self-sensing capacity without additional commercial sensors, and it has been applied in many electromechanical systems [15]. Yang et al. [15,16] developed a piezo-driven gripper based on parallelogram mechanism and double-rocker mechanism. Two groups of strain gauges are attached to the flexure hinges and beams to measure the position and the gripping force. Xu [17] designed a compact compliant stage with a large rotational range by utilizing multistage compound radial flexures. The driving torque acting on the stage and the resulting rotational angle were detected via a strain-gauge sensor attached to the leaf flexure. Kam et al. [18] proposed a new strain gauge-based sensing scheme to measure the displacement of ionic polymer-metal composite actuators for water applications. Taking the above into account, in this paper a brand-new slender robot with a flexible body and a compliant manipulator is developed for disaster research and rescue. Firstly, the mechanical structure of the compliant manipulator is described, which consists of a rotary joint, two bending joints and a compliant gripper. The position and clamping force of the gripper can be detected by several strain gauges sensors attached at the beams with large strain. And then, a hybrid force/position control scheme is developed to regulate both gripper tip position and clamping force and, furthermore, achieve smooth interaction. The force control is realized via a cascaded proportional-integralderivative (PID) controller, whereas the position control is realized via a fuzzy sliding mode controller (FSMC). Finally, the effectiveness of the presented mechanical structural design, sensing strategy and control algorithm are demonstrated by trajectories tracking experiments and closing-clamping-opening operation of gripper. 2. Overview of the rescue robot system The overall architecture of the developed rescue robot system is depicted in Fig. 1. The rescue robot system is mainly composed of a driving system located at the proximal end, a slender flexible tube with a diameter of 40 mm and a length of 3 m, an automatic insertion system consisting of a crawling mechanism and a pushing/pulling mechanism, and a compliant manipulator mounted at the distal end of the slender tube. A micro-camera integrated with audio intercom system (ASK-40, Mensa Inc.) is installed on the front of the manipulator for the purpose of collecting the environment information and guiding the rescue operation. The rescuer is able to get the view of micro-camera from the monitor and communicate with the survivors through intercom to acquire their real-time conditions and requirements. There are three degrees of freedom (DOFs) adjusting the orientation and posture of compliant manipulator and an extra DOF
Fig. 1. Schematic diagram of the tendon-sheath actuated rescue robot system.
performing grasping operation. The food, water, fresh air, and emergency medicine can be transferred to survivors through the slender flexible tube. In some situations, the manipulator needs to feed the survivors, or even provide simple treatment to the injured victims. The CAD model and geometrical dimensions of the compliant manipulator is presented in Fig. 2. As can be observed, the tendon-sheath driven manipulator can be basically divided into three parts: the gripper, the rotary component and the bending component. The gripper will clamp and grasp the object if the tendon attached to the internal beam of gripper is pulled. On the contrary, because of the recoverable property of elastic deformation, the gripper will automatically open if the tendon is relaxed. The rotary joint plays a significant role during rescue operation. When the tendon fixed on the groove of driving pulley is pulled, the rotary component is able to perform the rolling movement in the range of ± 90°. The bending component consists of two C-shape compliant bending units, which are mutually orthogonal and can perform the yawing movement and the pitching movement in the range of ± 45° respectively. Fig. 3 illustrates the photographs of the compliant manipulator performing different actions [19]. The tendon sheath transmission mechanisms packaged inside the slender flexible tube are capable of providing remote power transmission through the narrow tortuous space. The tendons utilized in the robot system are stainless steel wires with a diameter of 0.8 mm. The sheaths are tightly wound springs made from alloy steel wire and having an inner diameter of 1.2 mm. With the tendon sheath transmission mechanisms, all of the active joints and the gripper of manipulator are actuated by the servo motors placed in the base box of driving system. Therefore, the actuation system can be separated from the end-effector. It is beneficial to simplify the mechanical structure design and achieve low body weight and small dimension. A worm driven crawling mechanism is connected between the compliant manipulator and the slender tube to insert the rescue system into the crevices of ruins. In addition, the pushing/pulling mechanism located on the tail of the tube is actuated via a stepper motor and capable of providing pulling force and pushing force to the tube via the action of friction. If the rescue system is trapped in the ruins, it can be pulled back easily with the cooperation of crawling mechanism and pushing/pulling mechanism. The detailed descriptions of the automatic insertion system have been introduced in our previous researches [20,21]. Each servo motor is equipped with a pulley to its shaft to convert driving toque into pulling force. The driving system is controlled via a microcontroller (C8051F040, Silicon Labs Inc.) which can process data and perform computation internally. All the sub-control systems are connected together to communicate with the host laptop computer (ThinkPad-E450, Lenovo Inc.) via a CAN bus network.
Q. Wu et al. / Mechatronics 46 (2017) 143–153
145
Fig. 2. CAD model of the compliant manipulator. (a) Integral structure; (b) Gripper; (c) Bending component; (d) Rotary component. The gripper is driven by tendon-1 and sheath-1. The bending joint-1 is driven by tendon-2, tendon-3, sheath-2, and sheath-3. The bending joint-2 is driven by tendon-4, tendon-5, sheath-4, and sheath-5. The rotary joint is driven by tendon-6, sheath-6, and sheath-7.
3. Calibration and modeling of the compliant gripper The sensing detection method and the kinematic and dynamic characteristics of the compliant manipulator are far more complicated when compared with those of the traditional rigid robot manipulator. This paper is mainly focused on researching the calibration method, modeling approach, and control strategy of the developed compliant gripper. The compliant rotary component and bending component previously mentioned can be analyzed and studied in similar ways. 3.1. Development and calibration of strain gauge sensors
Fig. 3. Photographs of the compliant manipulator performing different actions. (a) Grasping movement; (b) Yawing movement; (c) Pitching movement; (d) Rolling movement.
For the purpose of measuring the gripper displacement and clamping force without additional nonintegrated sensors, two groups of high-precision resistance strain gauges were attached to the large-strain positions of the gripper [22]. The finite element analysis (FEA) of gripper was performed using SolidWorks/Simulation software package to determine the target locations. The material parameters of compliant gripper, which made of beryllium bronze, are specified as follows: elasticity modulus = 128 GPa, yield strength = 1035 MPa, density = 8260 kg/m3 , and Poisson’s ratio = 0.33. During FEA simulation, a pulling force with a value of F = 10 N was applied to tendon. The base of gripper was set to be immobilized. The FEA result with 68,600 elements is shown in Fig. 4. As can be observed, the induced large-strain positions appear around the inner ends of the gripper, which are marked as point a, point b, point c and point d. The maximum stress is about 240 MPa, which is smaller than the yield strength. Thus, to achieve self-sensing capacity, four strain gauge sensors are symmetrically arranged on these large-strain positions of elastic beams, which are connected into a double-arm bridge to compensate temperature variation and linearize output. The gripper with
146
Q. Wu et al. / Mechatronics 46 (2017) 143–153
Fig. 4. FEA results of the gripper when a pulling force with a value of 10 N is applied to the tendon.
Fig. 5. Schematic diagram of the gripper utilizing strain gauges to measure clamping force and displacement.
strain gauges is shown in Fig. 5. The strain gauges were divided into two groups according to the corresponding detected objects. More specifically, the strain gauge group I is capable of detecting the displacement of the gripper, while the strain gauge group II is capable of detecting the clamping force. Two high precision pressure sensors (FlexiForce A201, Tekscan) were attached to the top and bottom surfaces of the clamped object to obtain the actual clamping force. Besides, a linear position sensor (KTC-1B, MIRAN) was connected to one of elastic beam of gripper. It should be noted that, the force required to drive the pushrod of the linear position sensor is less than 0.05 N and, thus, can be neglected during practical operation. Calibration experiments were conducted to establish internal relations between the output signals from strain gauges and the displacement and clamping force of gripper. The voltages of strain gauges I and strain gauges II were compared with the outputs of high-precision position sensor and pressure sensor. The experimental setup of the compliant gripper control system was developed in the MATLAB/Simulink (Mathworks Inc.) environment with the Real-Time-Workshop (RTW) kernel, as depicted in Fig. 6. The platform mainly consists of a servo AC motor (SGDV-2R8A0, YASKAWA Inc.) as the actuator, two industrial personal computers (IPC-610H, Advantech Inc.) as the host computer and the real-time target, a tendon-sheath transmission system utilized to delivered the pulling force to the gripper with four strain gauge sensors, and two high precision pressure sensors (FlexiForce A201, Tekscan) and a linear position sensor (KTC-1B, MIRAN) to measure the clamping force and displacement of gripper. The tendon-sheath transmission system includes a flexible outer sheath and an inner tendon. The proximal and distal ends of tendon were attached to a pulley mounted on the motor shaft, while the distal end of tendon was attached to the gripper. Then the pulling force from the motor can be transmitted to the gripper by displacement between the tendon and sheath. The inner tendon of the platform is stainless steel wire with a diameter of 0.8 mm and a length of 0.8 m.
Fig. 6. Experimental setup of the compliant gripper control system. (a) Schematic diagram; (b) Photograph of the experimental configuration.
The outer sheath has an inner diameter of 1.2 mm and a length of 0.6 m. The analog sensor signals were first amplified by a transistor made voltage amplifier, and then acquired and processed by an A/D data acquisition card (PCL-818HD, Advantech Inc.). A fifthorder low-pass Bessel filter with a cut-off frequency of 35 rad/s was made use to filter the acquired analog signals. The digital motion control command, generated in the real-time target, were transformed into analog output signal via a D/A card (PCL-726, Advantech Inc.) and transmitted to the motor servo driver with a sampling rate of 10 kHz. In the calibration experiments, the calibration ranges of the strain-gauge displacement sensor and force sensor are about [0 mm, 11 mm] and [0 N, 19 N] with 20 sampling points, respectively. The calibration results are shown in Figs. 7(a) and 8(a), where the actual gripper displacement and clamping force (vertical axis) are plotted against the voltages of strain gauges (horizontal axis). It can be clearly observed that the experimental results show approximately linear relationship between the strain signals and the corresponding measuring parameters. The least square method was applied to conduct the multipoint linear fitting process. As a result, the following relationships hold:
x = 8.23VI − 0.22 mm
(1)
Fc = 12.31VII − 10.59 N
(2)
Q. Wu et al. / Mechatronics 46 (2017) 143–153
147
Fig. 7. Calibration results of the strain-gauge displacement sensor. (a) The relationship between the voltage of strain gauges I and the gripper displacement; (b) Time history and (c) histogram of the noise of strain gauges I.
Fig. 8. Calibration results of the strain-gauge force sensor. (a) The relationship between the voltage of strain gauges II and the clamping force; (b) Time history and (c) histogram of the noise of strain gauges II.
where x and Fc denote the displacement and clamping force of gripper; VI and VII represent the detected output voltages of strain gauges I and strain gauges II. The measurement white noises of strain gauges with zero voltage inputs within five seconds were acquired and depicted in Figs. 7(b) and 8(b). Meanwhile, the histograms of the measurement noises were shown in Fig. 7(c) and 8(c). We can see that the measurement white noises approximately followed normal distribution. More specifically, the mean and standard deviation of the white noises of strain gauges I were 0.0049 mm and 0.0946 mm, which reveals that 95% of the noise values were located in the interval of 0.0049 ± 0.1854 mm around the mean value. On the other hand, the 95% confidence interval of the white noises of strain gauges II was 0.0305 ± 0.3483 N. 3.2. System modeling and parameters identification According to the existing literatures [23–24], the compliant manipulating gripper system (including the compliant gripper and the tendon-sheath transmission system) can be simplified into a massspring-damper system with a dynamic model shown as follows:
Mx¨ (t ) + Bx˙ (t ) + Kx(t ) + P (t ) = τ (t )
(3)
where t denotes the time variable; x denotes the displacement of gripper; M, B, and K represent the parameters of mass, damping, and stiffness of gripper respectively; P(t) describes the lumped effects of system friction, tendon-sheath elongation and hysteresis, and external disturbance; τ (t) is output torque of driving motor. Open-loop frequency response experiments were conducted to identify the dynamics parameters of the compliant gripper system
with tendon sheath actuation. The dynamic model reveals the relationship between the output driving torque of servo motor and the displacement, velocity and acceleration of gripper. Tendon-sheath actuation system is capable of providing remote power transmission. However, due to the tendon compliance and the friction between tendon and sheath, there exist many undesirable nonlinear problems, such as backlash, dead zone, hysteresis, slacking and direction-dependent behavior. Moreover, the transmission property is affected by the total bending angle of system, which may vary during the rescue operation [25]. In our previous research [26], the transmission characteristic and model identification approach of the tendon-sheath actuation system have been described and analyzed in detail. This section is focused on analyzing the dynamic model of compliant gripper. In order to minimize the effects from tendon-sheath system, the total tendon bending angle of the experimental setup was set to zero during the identification experiments, as shown in Fig. 6(b). Therefore, the effect of the lumped uncertainties and tendon-sheath system, i.e., P(t), can be neglected during parameters identification. The servo motor was set to run in torque control mode and required to provide sinusoidal driving torque with constant amplitude of 0.5 Nm and various frequencies ranging from 0.1 to 40 Hz. Fig. 9 illustrates the Bode diagram of open-loop frequency response of the compliant gripper, which describes the transfer from the input driving torque to the output gripper displacement. From the magnitude and phase characteristics, it can be observed that dynamic model of gripper can be approximated as a second order linear model if the driving frequency is lower than 10 Hz. More specifically, the system contains a resonant peak around 6 Hz with a magnitude value of 28.33 dB, and
148
Q. Wu et al. / Mechatronics 46 (2017) 143–153
Fig. 10. Schematic diagram of the Cascaded PID controller for clamping force.
Fig. 9. Bode diagram of frequency response of compliant gripper. (a) Magnitude characteristic; (b) Phase characteristic.
the magnitudes in the frequencies below 1 Hz are about 25 dB. According to the experimental data at frequencies lower than 10 Hz, the second order model can be well fit by using the discrete data fitting tool of MATLAB (MathWorks, Inc.). The identified dynamic model can be described by
G (s ) = =
x (s ) 1 = τ (s ) Mˆ s2 + Bˆs + Kˆ 18.94
4.091 ×
10−4 s2
+ 1.576 × 10−2 s + 1
(4)
ˆ , Bˆ, and Kˆ represents the estimated parameters of mass, where M damping coefficient, and stiffness of gripper respectively. The experimental results and fitting results are compared in Fig. 9. It can be seen that the fitting results of the second order model matches the experimental results well in the frequencies lower than 10 Hz. For the frequency higher than 10 Hz, a higher order model is required to describe the dynamic model of gripper. However, since the necessary bandwidth of rescue operation is lower than 10 Hz, the developed simplified dynamic model can be applied in the control strategy of practical application.
was made use to restrict the maximum driving force and guarantee operation safety. The proportional gain Kp , integral gain Ki , and derivative gain Kd of the outer loop PID controller and inner loop PID controller were estimated via Ziegler-Nichols method [27] and optimally tuned by intensive tests to improve control accuracy and stability. Force tracking experiments were carried out using the real-time experimental setup (Fig. 6). Experimental performance of the developed cascade PID controller was compared to that of a PID controller. The gripper was required to grasp the target object and control the clamping force to follow a sinusoid wave signal with time-varying frequency and peak-to-peak amplitude. The first and last four seconds of the sinusoid signal saw a frequency of 0.25 Hz and an amplitude of 20 N. From the moment t = 4 s to t = 7 s and t = 11 s to t = 14 s, the frequency and amplitude changed to 0.33 Hz and 16 N. From the moment t = 7 s to t = 11 s, the frequency and amplitude turned to 0.5 Hz and 12 N, respectively. The selected control parameters of the cascade PID controller and PID controller are presented in Table 1. The results of force tracking experiments with different control strategies are compared and depicted in Fig. 11(a) and (b). As can be observed, the tracking performance of the cascade PID controller goes much better than that of the PID controller. The root mean square errors (RMSE) decline from 2.016 N (PID) to 0.821 N (cascade PID). Regarding the peak to peak errors, the cascade PID algorithm gives very small value, i.e., 1.586 N, in comparison with the value of PID controller, i.e., 3.975 N. The histogram of the force tracking errors with cascade PID controller is plotted in Fig. 11(c). The 95% confidence interval of the force tracking errors was −0.0129 ± 1.6091 N. Therefore, the results of force tracking experiment demonstrate the efficacies of the strain-gauge force sensor and the cascade PID controller. 4.2. Position control strategy
4. Control methods and experimental validation 4.1. Force control strategy The control of the clamping force of gripper is realized by utilizing a cascade PID controller, which is capable of responding to the external disturbance rapidly and improving trajectory tracking performance. The schematic diagram of the controller is shown in Fig. 10. The analog output signals from strain gauges II are firstly filtrated via a Bessel low pass analog filter with a cut-off frequency of 35 rad/s, and then converted into the clamping force value F according to the relationship shown in Eq. (2), and finally compared with the desired force Fd . The outer loop PID regulator operates the force error between Fd and F, and the output is added with the comparison error of the time rate of changes of desired force and measured force and, after that, sent to the inner PID regulator. Finally, the inner PID regulator outputs a desired voltage with a saturation interval of ± 5 V to servo controller. The servo motor was set to run in torque control mode, and the saturation module
In this section, a FSMC law is proposed to regulate the displacement of gripper. The control objective is to develop an applicable robust control strategy with favorable performance, so that the gripper can follow expected reference position trajectory under undesirable influence of bounded uncertainties. First, Eq. (3) can be refined as:
τ (t ) − MB x˙ (t ) − MK x(t ) − MP (t ) = A p τ (t ) + B p x˙ (t ) + C p x(t ) + D p (t )
x¨ (t ) =
1 M
(5)
where Ap = 1/M, Bp = −B/M, Cp = −K/M, and Dp = −P/M. Consider Eq. (5) with unpredicted model parameter uncertainties for actual gripper, the system dynamic model can be rewritten as:
x¨ (t ) = (A p + A )τ (t ) + (B p + B )x˙ (t ) + (C p + C )x(t ) + D p (t ) = A p τ (t ) + B p x˙ (t ) + C p x(t ) + L(t ) (6)
Q. Wu et al. / Mechatronics 46 (2017) 143–153
149
Table 1 The parameters of cascade PID controller. Parameters
Kp Ki Kd
Cascaded PID controller
PID controller
Outer loop PID controller
Inner loop PID controller
1.2 0.8 0.2
3.6 2.0 0.5
4.1 2.5 1.0
Fig. 11. Results of force tracking experiment. (a) Comparisons of desired trajectory and experimental trajectory; (b) Time history and (c) histogram of the force tracking errors.
L(t ) = Aτ (t ) + Bx˙ (t ) + Cx(t ) + D p (t )
(7)
where A, B, and C represent the uncertainties of system parameters; L(t) denotes the lumped uncertainty, whose boundary is assumed to be given as:
|L(t )| < η
(8)
where η represents a predetermined positive constant. A sliding model controller with a PD sliding surface is developed to enforce the system state trajectory to approach to the sliding surface and ensure expected closed-loop control performance and specifications. The position, velocity and acceleration tracking errors are defined as:
e(t ) = xd (t ) − x(t )
(9)
e˙ (t ) = x˙ d (t ) − x˙ (t )
(10)
e¨ (t ) = x¨d (t ) − x¨ (t )
(11)
where xd (t), x˙ d (t ) and x¨d (t ) are the desired position, velocity and acceleration. The switching function of the sliding surface is defined in the state-space R2 and expressed as the following equation:
S(t ) = e˙ (t ) + λe(t )
(12)
where λ is a positive constant of proportional gain. The expression for the deviation of the sliding variable can be shown as:
S˙ (t ) = e¨ (t ) + λe˙ (t )
(13)
Then, inserting Eqs. (6) and (11) into (13), the following equation can be obtained:
S˙ (t ) = x¨d (t ) + λe˙ (t ) − A p τ (t ) − B p x˙ (t ) − C p x(t ) − L(t )
(14)
In the conventional design of SMC system, the control law U(t) is basically composed of two terms, i.e. the equivalent control term
Ueq (t) and the hitting control term Uhit (t), and can be represented in the general form as follows [28]:
U (t ) = Ueq (t ) + Uhit (t )
(15)
The equivalent control term Ueq (t), which determines the dynamic performance of system on the sliding surface, can be derived as the solution of S˙ (t ) = 0 without considering any lumped uncertainty, i.e. L(t) = 0. Thus, from Eq. (14), we can get:
Ueq (t ) =
Bp Cp 1 λ e˙ (t ) − x˙ (t ) − x(t ) x¨ (t ) + Ap d Ap Ap Ap
(16)
However, the equivalent control effort alone cannot ensure the control performance if the initial system state does not lie on the sliding surface or parameters variations and undesirable disturbance occur during motion. Therefore, the hitting control term Uhit (t) should be applied to eliminate the effect of perturbations and drive the system toward the reference trajectory. As it is well known, the sign function can be utilized to establish the hitting control law and ensure the globally stable equilibrium of system. However, it may lead to heavily chattering phenomenon and high-frequency unmodeled dynamics. Thus, in order to mitigate the undesirable chattering problem, the hitting control law is realized by using a fuzzy logic inference mechanism in our research. The overall schematic diagram of the FSMC is illustrated in Fig. 12. The analog output signals from strain gauges I were filtrated via a Bessel low pass analog filter with a cut-off frequency of 35 rad/s, and converted into the gripper displacement x according to Eq. (1). The hitting control law can be represented as:
Uhit (t ) = FC(S(t ) )
(17)
where FC(·) denotes the proposed nonlinear fuzzy controller with the sliding variable S(t) and the hitting control term Uhit (t) as the input linguistic variable and output linguistic variable, respectively. The fuzzy logic controller is developed by utilizing the Mamdani inference method [29] and applied to the SMC algorithm for online turning of Uhit (t). In the fuzzification process, the input of fuzzy logic is scaled and mapped into three trigonometry/trapezoidal shape membership functions, namely, negative (N),
150
Q. Wu et al. / Mechatronics 46 (2017) 143–153
Here 0 ≤ A(rj ) ≤ 1 represents the firing strength of rule j. Moreover, the sum of all firing strengths is equal to one, i.e. A(r1 ) + A(r2 ) + A(r3 ) = 1. r1 = −r, r2 = 0 and r3 = r denote the center of the membership functions NE, ZE, and PE, respectively. r is a positive constant. And then, the value of hitting control term can be analyzed according to the value of sliding variable S(t). According to Fig. 13, the following four situations shown in Table 3 can be obtained: From the above analyzing results of different situations, it can be observed that 0 ≤ S(t)[A(r3 ) − A(r1 )] = |S(t)|·|A(r3 ) − A(r1 )|. Furthermore, the entire control law shown in Eq. (15) can be represented as:
U (t ) = Ueq (t ) + r [A(r3 ) − A(r1 )]
Fig. 12. Schematic diagram of the FSMC strategy for gripper displacement.
(19)
For the purpose of demonstrating the system stability with the proposed FSMC scheme, the following Lyapunov candidate function is proposed [30]:
V (t ) =
S2 (t ) 2
(20)
Differentiating S(t) with respect to time and utilizing Eqs. (14)– (16) and (19) gives:
V˙ (t ) = S(t )S˙ (t ) = S(t )[x¨d (t ) + λe˙ (t ) − A pUeq (t ) − A pUhit (t ) − B p x˙ (t ) − C p x(t ) − L(t )] = −S(t )A pUhit (t ) − S(t )L(t ) Fig. 13. The input membership function for S(t). N: negative; Z: zero; P: positive.
≤ −A p r |S(t )||A(r3 ) − A(r1 )| + |S(t )||L(t )| = −|S(t )|[r A p |A(r3 ) − A(r1 )| − |L(t )|]
Fig. 14. The output membership function for Uhit (t). NE: negative effort; ZE: zero effort; PE: positive effort. Table 2 The fuzzy control rules for Uhit (t). Rule
S(t)
Uhit (t)
1 2 3
N Z P
NE ZE PE
zero (Z) and positive (P) respectively, as shown in Fig. 13. The control actions of fuzzy logic are divided into three fuzzy sets, namely, negative effort (NE), zero effort (ZE) and positive effort (PE) respectively, as shown in Fig. 14. Each combination of scaled input and membership function combination activates one control action according to the inference rules. The fuzzy linguistic rules are defined in Table 2. The center-of-gravity method is chosen as the defuzzification strategy to compute the output variable of fuzzy logic. It guarantees high control sensitivity to the changes of input signals and, meanwhile, avoids time-consuming calculation process. The output value can be calculated as follows:
3 j=1
Uhit (t ) = 3
r j A (r j )
j=1
A (r j )
= r1 A ( r1 ) + r2 A ( r2 ) + r3 A ( r3 )
(18)
(21)
If we select r > |L(t )|/[A p |A(r3 ) − A(r1 )|], the reaching condition V˙ (t ) = S(t )S˙ (t ) < 0 is always satisfied. It should be noted that V(t)is positive definite and V˙ (t ) is negative definite. Besides, when S(t) tends to infinity, V(t)also approaches to infinity. Consequently, the closed loop control system is globally asymptotically stable. The position tracking error state gradually converges to the sliding surface S(t) = 0 as t → ∞. Position tracking experiment was conducted in the experimental setup shown in Fig. 6. Experimental performance of the developed FSMC algorithm was compared to that of a conventional SMC algorithm [31]. The desired gripper displacement was defined to follow a sinusoid wave trajectory with time-varying frequency and peak-to-peak amplitude. The first eight seconds of the sinusoid trajectory had a frequency of 0.25 Hz and an amplitude of 10 mm, which then changed to 0.33 Hz and 12 mm from the moment t = 8 s to t = 14 s. In the last six seconds, the frequency and amplitude changed to 0.5 Hz and 8 mm, respectively. It needs to be pointed out that, during the tracking experiment, the feedback signals applied in the position controller came from the strain-gauge sensors, while the actual gripper displacement was detected via the highprecision position sensor. The parameters of the proposed FSMC and conventional SMC algorithms were empirically adjusted to achieve superior control performance. During the experiments, the parameters of FSMC were set to λ = 6.8, S1 = −1.2, S2 = 1.2, r = 3.5. The dynamic model parameters of gripper have been identified in the previous research. The results of position tracking experiments with different control strategies are compared in Fig. 15(a) and (b). It can be observed that the tracking performance of FSMC has turned out to be far more desirable. More specifically, the root mean square errors (RMSE) decline from 1.216 mm (SMC) to 0.383 mm (FSMC). The peak to peak error of the FSMC algorithm (i.e., 1.005 mm) is smaller than the value of SMC algorithm (i.e., 2.694 mm). Besides, according to Fig. 15(b), the chattering level can be significantly reduced with FSMC algorithm. The histogram of the position tracking errors under FSMC is presented in Fig. 15(c).
Q. Wu et al. / Mechatronics 46 (2017) 143–153
151
Table 3 The values of hitting control term in different situations. Condition
Activated rules
A(r1 )
A(r2 )
A(r3 )
Uhit (t)
S2 < S(t) 0 < S(t) ≤ S2 S1 < S(t) ≤ 0 S1 > S(t)
Rule Rule Rule Rule
A(r1 )= 0 A(r1 )= 0 0 < A(r1 ) ≤ 1 A(r1 )= 1
A(r2 )= 0 0 < A(r2 ) ≤ 1 0 < A(r2 ) ≤ 1 A(r2 )= 0
A(r3 )= 1 0 < A(r3 ) ≤ 1 A(r3 ) = 0 A(r3 )= 0
Uhit (t) = r3 Uhit (t) = r2 Uhit (t) = r1 Uhit (t) = r1
3 2 and rule 3 1 and rule 2 1
A(r3 ) = r A(r2 ) + r3 A(r3 ) = r A(r3 ) A(r1 ) + r2 A(r2 ) = −r A(r1 ) A(r1 ) = −r
Fig. 15. Results of position tracking experiment. (a) Comparisons of desired trajectory and experimental trajectory; (b) Time history and (c) histogram of the position tracking errors.
The tracking errors complied with the normal distribution approximately and the 95% confidence interval was 0.0059 ± 0.7524 mm. The results of position tracking experiment verify the accuracy of the developed strain-gauge position sensor and the effectiveness of FSMC strategy. 4.3. Hybrid force/position switching control strategy In the aforementioned sections, the cascaded PID controller and FSMC law have been developed to guarantee the effectiveness of the force tracking control and position tracking control of gripper. This section deals with a hybrid force/position switching control strategy utilized to coordinate the relationship between force control and position control during operation. In order to perform a completed manipulation task, the handling procedure of the compliant gripper falls mostly into three steps: closing step, clamping step, and opening step. Initially, there is no pulling force acting on the inner tendon, and the two elastic beams of gripper maintain in their home positions. In the duration of closing step, the gripper is driven to approach to the object and execute the grasping action. It comes to the clamping step after the target object contacts with the gripper and is removed to a desired location. In this phase, the clamping force varies with the actuation of inner tendon, and the gripper displacement stabilizes on the maximum value. The opening step occurs after the object is loosened and placed on the destination. In this phase, the clamping force decreases to zero, while the gripper displacement is reduced under the tendon-sheath actuation. The analysis of operational process indicates that the position control performance is crucial to the closing step and opening step, while the accurate force control strategy should be executed during clamping procedure. Therefore, a hybrid force/position switching control strategy, which is integrated with the previously developed cascaded PID force controller and fuzzy sliding mode position controller, is proposed to ensure smooth transition between different working steps and achieve satisfied control results, as illustrated in Fig. 16. The switching criteria are established based on the detected clamping force. More specifically, the hybrid controller will be switched to force control mode if the gripper contacts with the object and the clamp-
Fig. 16. Schematic diagram of the hybrid force/position switching control strategy.
ing force is larger than the predefined threshold value Fs . Otherwise, if the clamping force is equal to or smaller than the threshold value Fs , the hybrid controller will be switched to the position control mode to make the gripper track the reference position trajectory accurately. The threshold value is determined according to the physical characteristics of the grasped object and the requirements of practical application. Experimental investigations were conducted to validate the effectiveness of the hybrid force/position switching control scheme and evaluate the performance of the grasping process. Firstly, the elastic beams of gripper were expected to move towards the target object with a constant velocity of 2 mm/s during the closing step. And then, after the gripper contacts with the object, the clamping force was required to maintain at the value of 8 N during the clamping step. The threshold value Fs of the switching criteria was set as 0.5 N. Finally, during the opening step, the desired clamping force was reduced to 0 N, while the elastic beams were expected to move away from the object and return to the initial positions in 5 s. In addition, the reference position trajectory of returning movement was defined as a cubic polynomial with continuous velocity. The predetermined trajectories of gripper displacement and clamping force and the experimental results are shown in Fig. 17(a) and 17(c). As can be observed, the first 4.8 s saw the closing of gripper, and the maximum displacement is about 9.6 mm. From t = 4.8 s to t = 10 s, the value of clamping force was regulated by
152
Q. Wu et al. / Mechatronics 46 (2017) 143–153
the force controller and kept around 8 N. The last 5 s saw the opening of gripper under the regulation of position controller. The position and force tracking errors are presented in Fig. 17(b) and (d) respectively. The RMSE of position and force tracking errors were calculated to be 0.268 mm and 0.385 N, which were about 3.68% and 8.34% of the RMS values of the desired position and force trajectories. Furthermore, the relationship between gripper displacement and clamping force are shown in Fig. 17(e). It is found that there was no clamping force during the closing and opening processes. The gripper displacement was kept in its maximum value while the object was grasped with a time-variable clamping force, mainly due to the high structural stiffness of the object and compliant property of the developed gripper. The experimental results demonstrate that the developed hybrid force/position control law is capable of regulating the gripper position and clamping force and achieving smooth interaction. 5. Conclusion and future works This paper dealt with the development and research of a long and slender rescue robot with compliant manipulator, which can work in the narrow and confine space of ruins for disaster research and rescue. Firstly, the major mechanical structure and control system of the tendon-sheath actuated rescue robot were introduced. And then, the research was mainly focused on the analysis and control of the compliant gripper. With the aim of measuring the gripper displacement and clamping force, two groups of resistive strain gages were attached to the large-strain positions of gripper under external tendon pulling force according to FEA simulation results. The sensors calibration and model identification were carried out based on the experimental setups established in Matlab/ RTW environment. In order to improve the operation performance of the gripper, a cascaded PID controller and a FSMC algorithm were developed to ensure the force and position tracking accuracies. In addition, a hybrid force/position control scheme was presented to coordinate the relationship between force control and position control while executing the object grasping task. Finally, the effectiveness of the developed force/position sensing approach and control algorithms were validated by trajectories tracking experiments with time-varying frequency and amplitude and closingclamping-opening operation of gripper. Future works will be devoted to further improve the proposed force/position control algorithm with the aim of efficiently reducing the tracking errors and achieving more complicated manipulation tasks. The dynamic calibration will be conducted to improve the measure accuracy of selfsensing system. Besides, the modeling and control of the entire compliant manipulator will be researched and applied to rescue application. These works are currently underway. Acknowledgments This research has been supported by the Natural Science Foundation of Jiangsu Province of China (BK20170783), the China Nation Nature Science Foundation (51575100 and 51575256), the NUAA Fundamental Research Funds (NS2013042), the NUAA Scientific Research Funds (90YAH16085). References Fig. 17. Results of the hybrid force/position tracking control experiments. (a) Gripper displacement tracking result; (b) Displacement tracking error; (c) Clamping force tracking result; (d) Force tracking error; (e) Relationship between gripper displacement and clamping force.
[1] Jang H C, Lien Y N, Tsai T C. Rescue information system for earthquake disasters based on MANET emergency communication platform. In: Proceedings of the 2009 International Conference on Wireless Communications and Mobile Computing: Connecting the World Wirelessly; 2009: 623–627. [2] Murphy R, Tadikoro S, Nardi D, et al. Search and Rescue Robotics. In: Springer handbook of robotics, part F: field and service robotics; 2007. p. 1151–73. [3] Li J, Wang X, Xi R. Tip force tracking control based on the tendon-sheath transmission model and 3D reconstruction of a long slender rescue robot. In: IEEE 14th international workshop on advanced motion control; 2016. p. 448–54.
Q. Wu et al. / Mechatronics 46 (2017) 143–153 [4] Arai M, Takayama T, Hirose S. Development of “Souryu-III” connected crawler vehicle for inspection inside narrow and winding spaces. In: IEEE/RSJ int. conf. intell. robots and sys; 2004. p. 52–7. [5] Arai M, Yoshinori T, Hirose S, Kuwahara H, Tsukui C. Development of ‘Souryu-IV’ and ‘Souryu-V:’ serially connected crawler vehicles for in-rubble searching operations. J Field Robot 2008;25:31–65. [6] Kitagawa A, Tsukagoshi H, Igarashi M. Development of small diameter active hose-ii for search and life-prolongation of victimsunder debris. J Robot Mech 2003;15:474–81. [7] Kazuyukilto HA. Development of rescue manipulator to search narrow space for victims. Artif Life Robotics 2008;13:331–5. [8] Chen L, Wang X. Design and preliminary experiment of a robot for searching interior rubble. In: international conference on systems, man, and cybernetics (SMC); 2011. p. 1956–61. [9] Liu L, Leonhardt S, Misgeld BJE. Design and control of a mechanical rotary variable impedance actuator. Mechatron 2016;39:226–36. [10] Campeau-Lecours A, Otis M, Belzile PL, Gosselin C. A time-domain vibration observer and controller for physical human-robot interaction. Mechatron 2016;36:45–53. [11] Lin J, Lin CC. Hybrid fuzzy position/force control by adaptive network-based fuzzy inference system for robot manipulator mounted on oscillatory base. J Vib Control 2015;21(10):1930–45. [12] Calanca A, Muradore R, Fiorini P. A review of algorithms for compliant control of stiff and fixed-compliance robots. IEEE-ASME T Mech 2016;21(2):613–24. [13] Kim HS, Kim IM, Cho CN, et al. Safe joint module for safe robot arm based on passive and active compliance method. Mechatron 2012;22(7):1023–30. [14] Khan SG, Herrmann G, Lenz A, Mubarak A, Pipe T, Melhuish C. Compliance control and human–robot interaction: part II—experimental examples. Int J Hum Robot 2014;11(03):1430 0 02. [15] Denkena B, Kiesner J. Strain gauge based sensing hydraulic fixtures. Mechatron 2016;34:111–18. [16] Yang Y, Wei Y, Lou J, et al. Development and precision position/force control of a new flexure-based microgripper. J Micromech Microeng 2015;26(1):015005. [17] Xu Q. Design of a large-range compliant rotary micropositioning stage with angle and torque sensing. IEEE Sens J 2015;15(4):2419–30. [18] Leang KK, Shan Y, Song S, et al. Integrated sensing for IPMC actuators using strain gages for underwater applications. IEEE-ASME T Mech 2012;17(2):345–55. [19] Kai W, Xingsong W. Preliminary experiments on soft manipulating of tendon-sheath-driven compliant joints. In: 2nd international conference on robot intelligence technology and applications; 2014. p. 503–15. [20] Zhang G, Wang X. Design and 3D reconstruction of a Tendon-Sheath-Driven searching and rescuing robot. In: IEEE international symposium on safety, security, and rescue robotics (SSRR); 2013. p. 1–6. [21] Zhang G, Wang X. Design of automatic insertion system for a tendon-sheath– driven searching and rescuing robot. In: 3rd international conference on mechatronics and applied mechanics; 2013. p. 175–8. [22] Leang KK, Shan Y, Song S, Kwang J. Integrated sensing for IPMC actuators using strain gages for underwater applications. IEEE-ASME T Mech 2012;17(2):345–55. [23] Xu Q, Li Y. Model predictive discrete-time sliding mode control of a nanopositioning piezostage without modeling hysteresis. IEEE T Contr Syst T 2012;20(4):983–94. [24] Xu Q. Design, testing and precision control of a novel long-stroke flexure micropositioning system. Mech Mach Theory 2013;70:209–24. [25] Chen L, Wang X, Xu WL. Inverse transmission model and compensation control of a single-tendon–sheath actuator. IEEE T Ind Electron 2014;61(3):1424–33. [26] Wu Q, Wang X, Chen L, Du F. Transmission model and compensation control of double-tendon-sheath actuation system. IEEE T Ind Electron 2015;62(3):1599–609. [27] Ziegler JG, Nichols NB. Optimum settings for automatic controllers. Trans. ASME 1942;64(11). [28] Wu Q, Wang X, Chen L, Du F, Xi R. Modeling and position control of a therapeutic exoskeleton targeting upper extremity rehabilitation. P I Mech Eng C-J Mec 2016; 0954406216668204. [29] Nagi F, Perumal L, Nagi J. A new integrated fuzzy bang–bang relay control system. Mechatron 2009;19(5):748–60. [30] Chen C, Ren CE, Du T. Fuzzy observed-based adaptive consensus tracking control for second-order multi-agent systems with heterogeneous nonlinear dynamics. IEEE T Fuzzy Syst 2015;24(4):906–15. [31] Young KK. Controller design for a manipulator using theory of variable structure systems. IEEE Trans Syst Man Cyber 1978;8(2):101–9.
153 Qingcong Wu received the B.S. and Ph.D. degrees in mechatronics engineering from Southeast University, Nanjing, China, in 2011 and 2016, respectively. He is currently a Lecturer with the College of Mechanical and Electrical Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing, China. His major research interests include robotics, tendon-sheath transmission theory, gravity balancing theory, and the application of exoskeleton to neuromuscular rehabilitation.
Xingsong Wang received the B.S. degree and M.S. degree from Zhejiang University, Hangzhou, China, in 1988 and 1991, respectively, and the Ph.D. degree from Southeast University, Nanjing, China, in 20 0 0, all in mechanical engineering. He was a visiting scientist at Concordia University, Canada (20 0 0.6–20 0 0.12, 20 01.9–20 02.3) and at Purdue University, USA (20 07.9–20 08.3), both at School of Mechanical Engineering. Currently, he is a full professor in the School of Mechanical Engineering and head of Department of Mechatronics at Southeast University. His current research interests include control theory with application in precision CNC machine tools, advanced mechatronics with applications in biomedical engineering, tendon-sheath transmission theory with application in rescue robots, and Mecanum-wheels based AGV systems designing. Bai Chen received the B.S. degree and Ph.D. degree from Zhejiang University, Hangzhou, China, in 20 0 0 and 20 05, respectively, all in mechanical engineering. Currently, he is a full professor in the College of Mechanical and Electrical Engineering at Nanjing University of Aeronautics and Astronautics. His current research interests include minimally invasive neurosurgery robot, virtual surgery system, force feedback control, interventional therapy.
Hongtao Wu received the B.S. degree from Yanshan University, Hebei, China, in 1982, and the M.S. degree and Ph.D. degree from Tianjin University, Tianjin, China, in 1985 and 1992, respectively, all in mechanical engineering. Currently, he is a full professor in the College of Mechanical and Electrical Engineering at Nanjing University of Aeronautics and Astronautics. His current research interests include parallel robot, robot kinematics, multibody system dynamics.
Ziyan Shao received the B.S. degree and M.S. degree from Yangzhou University, Yangzhou, China, in 2013 and 2016, respectively. Currently he is working toward the Ph.D. degree in the College of Mechanical and Electrical Engineering at Nanjing University of Aeronautics and Astronautics. His major research interests include exoskeleton, robot control, and robot dynamics.