Design of master-slave velocity tracking Upper-Limb Exoskeleton system and Feed-forward compensation control

 Abstract: In this paper, we propose a design approach of mirror driving master-slave upper limb exoskeleton based on inertia measurement unit (IMU) system and ZigBee wireless network transmission system. By integrating two gyroscopes on the master side, we can conduct measurement of rotation velocity and acceleration signal and control over a larger range of motion (ROM) of the exoskeleton. The angular velocity information is transmitted to the controller on the paretic side through ZigBee wireless module. Applying the Lagrangian dynamic equation and Geometric projection method, the 3-axes angular velocity are converted into the reference angular velocity control signal of four joint motors of the rehabilitation upper limb exoskeleton. Moreover, a novel control scheme based on inertia moment and gravity moment feed-forward compensation is proposed. The simulation results demonstrate that the controller can enhance the performance according to the responding speed and stability. Experimental results show that the designed scheme of the mirror driving master-slave upper limb exoskeleton system is completely feasible and


Introduction
Given the increasing demand for physical rehabilitation due to the coronavirus COVID-19 including increased patients of stroke and paralysis, robotic exoskeleton rehabilitation device is an emerging and crucial technology. An upper limb assistive exoskeleton can provide rehabilitation training and assist hemiplegic patients to restore some lost function due to neurologic injury. In the early stages of rehabilitation training, it is very difficult for hemiplegic patients to drag their upper limbs by themselves, so external assists are needed during the rehabilitation training process. A rehabilitation exoskeleton is designed to hold up and assist the impaired limb for patients in passive rehabilitation training. In the last two decades, numerous robotic systems have been developed to assist stroke survivors during the rehabilitation phase. Nathanael et al. [1] searched PubMed, ClinicalTrials, IEEE Xplore Digital Library, Science Direct, and Google Scholar databases, using different combinations of the following keywords: "upper-limb, robot, exoskeleton, rehabilitation, assisted, shoulder, elbow, wrist, arm, therapy, stroke, and training", and found that 30 different exoskeleton prototypes for neurorehabilitation of the upper limbs had been the subject of publications in the mainstream journals and conferences.
Massachusetts Institute of Technology (MIT) designed an end-point upper extremity rehabilitation robot named MIT-MANUS in 1992 [2], it can realize the compliant movement of the end point and provide visual feedback to the wearer through the computer game interface [3]. An exoskeleton named ARMin has been developed by the ·3· University of Zurich, Switzerland [4,5], the exoskeleton is installed on a mobile wheelchair to reduce the weight of the user, and a mechanical locking device is installed at each joint to prevent the movement of the exoskeleton from exceeding the safety range of the human body. Tsinghua University has developed a double linkage rehabilitation training device which can realize multimode load training [6]. Thieme et al. [7] found that mirror-image (master/slave drive mode) rehabilitation therapy has a better therapeutic effect than other treatment methods. Based on previous research, Stanford University has developed a Mirror Image Motion Enable (MIME) upper extremity exoskeleton, patients can control the motion of their impaired upper limbs by moving their healthy counterpart and complete mirror image bimanual movements [8,9]. Lum et al. [10] designed a mirror-image rehabilitation training device driven by surface electromyography(sEMG) signals which can be generated in the process of upper limb movement. Gautam et al. [11] proposed a mirror image human-computer interface system named Wear-A-BAN. The motion state of the upper limb is monitored in real time through the wireless body area network (WBAN) sensor installed on the healthy limb of the patient. However, this interactive mode cannot achieve independent rehabilitation training since it still needs physiotherapists to participate in the process. Another master/slave training device, designed by Beom et al. [12], collecting the motion data by using three attitude and heading reference system (AHRS) sensors which placed on the upper limb. Wang et al. [13] proposed a novel scheme based on an inertial measurement unit (IMU) for collecting position and attitude data of the healthy limb movement. In [14], Du et al. recorded and calculated the patient' joint angles using Kinect and IMUs while they moved during movement games. The study increases the accuracy of the movement detected by the Kinect and increases the processing rate of the IMUs. Zhao et al. [15] proposed a wearable body motion capture system based on IMU and IEEE 802.11b wireless communication and demonstrated its application in assistive exoskeleton.
Existed exoskeleton manipulators are usually designed based on position (trajectory) tracking control. The expected trajectory of exoskeleton motion is given before rehabilitation training and the control quantity is calculated from the position error and velocity error. PID controller is the most used control method because of high trajectory tracking performance, and it is easy to implement in most robotic systems [16]. Different nonlinear control strategies are used to track the reference trajectory, such as adaptive control [17][18][19], sliding mode control [20,21], PID feedback control, fuzzy logic control [22], fuzzy neural network control [23] and robust hybrid control [24], etc. These control strategies can be problematic because of complex calculations and unavailable full-state measurements. Mofid et al. [25] consider a fractional-order quadratic chaotic flow with nonhyperbolic equilibrium and proposes an adaptive sliding mode disturbance-observer, it has great effects on the fractional-order quadratic chaotic systems in the presence of external disturbances. Mobayen et al. [26] propose a combination of finite-time robust-tracking theory and nonlinear feed-back approach for chaotic systems which has high performance and could convergence in the finite time.
Exoskeleton manipulator is a strong coupling nonlinear system. The most important factor that affects the control effect is the moment of inertia and the moments of gravity vary greatly along with the change of position. It is necessary to perform feed-forward compensation control for inertia moment and gravity moment to accomplish the control purpose. For an industrial manipulator, the target of control is to make the end of the manipulator reach a certain position precisely while the control purpose of the rehabilitation exoskeleton manipulator is to drive or assist the patient's muscle activity simultaneously. Thus, a rehabilitation exoskeleton manipulator requires speed synchronization control rather than trajectory tracking control compared with industrial manipulators. Rehabilitation training devices should be as simple, convenient, and practical as possible, so the patients can do self-rehabilitation exercise handily at any time without the complex operation of parameter adjustment. It is the most effective way for patients to use their healthy arm driving another injured arm to do mirror-image rehabilitation exercises. In [27], researchers propose a design of composite nonlinear feedback control of master/slave nonlinear systems with time-varying delays and parametric uncertainties. Bouteraa et al. [28] propose a game-based hand rehabilitation system which allows patients to realize physical rehabilitation therapies based on Kinect's skeletal tracking features and the EMG sensor. Patient movement information and signals obtained from the developed exoskeleton device are used together to monitor the rehabilitation progress.
The best knowledge of the authors, the original contributions of this article are itemized as follows: a) A ZigBee wireless transmission module based mirror image driven upper limb exoskeleton system is considered. Compared with traditional rehabilitation exoskeleton, the proposed design makes it more convenient for patients to perform rehabilitation exercises due to lighter structure and less resistance. Compared with [15], the proposed transmission scheme is more suitable for low-power, low-speed, and short-range wireless communication application scenarios. b) An angular velocity synchronization tracking driven scheme is adopted to perform master-slave rehabilitation motion. The angular velocity signals of elbow and wrist joint are collected by gyroscopes to drive the paretic side simultaneously. A novel velocity tracking design fulfils the real-time response against disturbance. It is not necessary to obtain the position information or record the joint motion trajectory as mention in [13][14].
c) A torque feed-forward compensation controller method which applies angular velocity rather than force/trajectory as control quantity is proposed. The suggested control method shows great velocity tracking performance while maintaining good stability.
The rest of the paper is structured as follow. The system architecture is presented in Section 2. Section 3 introduces the main results including the calculation of dynamic parameters and angular velocity. The design of feedforward torque compensation controller is also presented in section 3. The experimental results and discussions are presented in Section 4. Finally, conclusions and future work are summarized in Section 5.

Design of angular velocity measurement and communication module
Two angular velocity measurement modules are planted on the elbow joint and wrist joint of the healthy upper limb respectively. Each module consists of two parts: data processing and data collection. Two measurement module chips and the main control chip of the ZigBee network coordinator use CC2530 chip manufactured by Texas Instruments (TI). CC2530 is the second-generation system-on-chip solution for 2.4GHz, IEEE802.15.4, ZigBee-2007/PRO, and RF4CE applications, it provides a robust and complete solution by integrating 8051 microprocessor, ZigBee RF transceiver and peripheral I/O interface which can meet the operation and interface requirements of acquisition terminals [29]. To enhance the signal strength and improve the reliability of ZigBee network, the CC2591 chip is added to the RF front-end to amplify the power of the signal and reduce noise. The maximum gain of LNA is 11dBm, the noise figure is 4.8dB, and the receiver sensitivity is 6dB [30]. The RF antenna adopts an inverted F antenna which is connected to the ANT port of CC2591. The parameters are set according to the rules announced by TI. The maximum gain is +3.3 dB, which can fully meet the requirements of the working frequency band of CC2530.
The exoskeleton system in our research only utilizes the rotation angular velocity of the healthy upper limb to drive the joint motor of the exoskeleton manipulator directly. It does not require the precise position and direction information, thus acceleration sensors and geomagnetic sensors are not applied in the system. Angular velocity data collection module uses a three-axis digital gyroscope L3G4200D chip which manufactured by STMicroelectronics (ST), it has three optional ranges (250/500/2000dps), two digital output interfaces (SPI/IIC) and 16-bit digital output, built-in low-pass and high-pass filters. The chip communicates with CC2530 chip through IIC bus specification by pulling up the CS pin of L3G4200D to VDDI/O to set L3G4200D into IIC mode, SCL and SDA pins of L3D4200D provide IIC interface to the outside of the module. The SCL port is connected to P1.4 of CC2530, and the SDA port is connected to P1.5 of CC2530. The Vcc and GND of CC2530 are connected to 3.3V and GND of L3G4200D respectively. It is necessary to set the PXDIR of each port's pin to 1 or 0. According to the IIC bus protocol, the data can be read and written to SCL and SDA at the specified time. The circuit diagram of the angular velocity data collection module is shown in Figure 1, and some peripheral circuits are simplified.

Figure 1 Circuit diagram of data collection module
ZigBee wireless communication network consists of three nodes: coordinator, router and sensor. Two sensor nodes are integrated in two angular velocity measurement modules. The main control computer acts as the coordinator node. The main control computer filters the angular velocity data measured by the gyroscope. The angle feedback data of the exoskeleton manipulator is fused to form the control instruction signal according to the designed control law.

Design of Exoskeleton Manipulator
We had developed a 6-DOF upper-limb exoskeleton manipulator in our previous work [31]. The 6-DOF of joints include shoulder joint abduction/adduction, shoulder joint flexion/extension and shoulder joint internal/external rotation, elbow joint flexion/extension, forearm supination/pronation and wrist joint flexion/extension. The structural design of the exoskeleton manipulator is shown in Figure 2. The length of the upper arm of the exoskeleton manipulator is adjustable, ranging from 220 to 320mm. The total weight of the upper arm, joint J3, and joint J4 is 4 kg. The length of the forearm can be adjusted from 250 mm to 350 mm. The total weight of the forearm joint J5 and wrist joint J6 is 3 kg. See Table 1 for the rotation angle range of each joint.

Calculation of Exoskeleton Manipulator Dynamic Parameters
For the convenience of calculation and practical needs, only the dynamic characteristics of the exoskeleton upper arm and forearm are studied since they have a major influence on the rehabilitation process. Based on the situation mentioned above, we make some assumptions as following: Assumption 1: The mass of the upper arm (m1 = 4kg) is concentrated at the center of the elbow joint(J4), and the mass of the forearm (m2 = 3kg) is concentrated at the center of the wrist joint (J6). Assumption 2: Only joints J1, J2, J3 and J4 are studied, the influence of the wrist joint and its rotation on the dynamic characteristics of the upper arm and forearm is ignored. Assumption 3: The momentum produced by coupling inertia, centrifugal force and Coriolis force is ignored due to the large mass of exoskeleton and the slow rotation velocity of joints.

Figure 3 Inertial frame of exoskeleton manipulator
The inertial frame is established as shown in Figure 3. The origin of coordinate O is the center point of joint J1, the direction of axis Oz is the same as that of the upper arm and pointing forward. The axis Oy is perpendicular to the ground and pointing upward. Points A and B represent the ·6· center point of elbow joint J4 and wrist joint J6 respectively. The link OA that represents the upper arm rotates an angle 1 around the axis Oz, then rotates 2 and 3 around the new axis OX1 (not shown in the figure) and axis OA respectively. The link AB that represents the forearm rotates 4 in the plane OAB with respect to the elbow joint J4, which is parallel to the axis OX1. The rotation angle of the front end of the forearm AB is 5 (not shown in the figure).
The position coordinates of points A and B are calculated as follows: where l1 and l2 are the length of the upper arm and forearm respectively.
= sin , = cos , ( = 1,2,3,4). For any mechanical system, the Lagrangian function L is defined as the difference between the total kinetic energy K and the total potential energy P, then the generalized Lagrangian dynamic equation is expressed as: In our case, the system dynamics equation described by Lagrangian function can be obtained as: where ( ,) is the Lagrange function, ( ,) is the total kinetic energy, ( ) is the total potential energy, is the driving torque of the i-th joint.

Calculation of Driving Angular Velocity
The angular velocities of the three axes ( 1 , 1 , 1 ) of upper arm rotation in the world coordinate system (WCS) are measured by gyr1, which is implemented on the elbow of the healthy upper limb. The measured angular velocities from gyr1 are used as the driving velocities of joint J1 and joint J2 of the exoskeleton manipulator, thus the angular velocities generated from gyr1 need to be converted into the angular velocities of joint motor rotation. The relationship between 1 , 1 , 1 and the rotational angular velocity of joints ̇1 , ̇2 and ̇3 can be calculated from:    (10) Therefore, the driving velocity of joint motor can be derived from equation (10) The driving angular velocities of J1 and J2 can be obtained by equation (11). However, the angular velocity ̇3 of joint J3 should not be calculated by equation (11) because the rotation angle 3 of the exoskeleton joint J3 represents the rotation angle of the upper arm around the direction of the impaired limb, since the healthy human upper limb wearing the gyr1 is a flexible body and the exoskeleton manipulator is a rigid body, the reading of ̇3 measured by the gyr1 cannot indicate the joint J3 rotation angular velocity. The angular velocities ̇1 and ̇2 can be expressed as: During the mirror-driven movement process, the movement of the forearm in the workspace can be divided into two parts: translational motion along with the upper arm (fixed elbow joint) and rotation relative to the upper arm. Since the two gyroscopes are implemented on the elbow and wrist joint respectively, the angular velocity values of 3-axes measured by gyr1 and gyr2 are the same in translational motion. The motion of the upper arm OA is divided into two rotations: rotation around the axis OA and the rotation around the elbow joint J4 in the OAB plane formed by the forearm AB and the upper arm OA. The principle is the same as that of the upper arm rotating around the shoulder joint. The principle of driving joint J3 is the same as that of driving joint J1; and the driving scheme of joint J4 is the same as that of driving joint J2. The differences are as follows: (1) The axis is different. The upper arm OA rotates angle 1 relative to the axis OZ, but the forearm AB rotates 3 around the axis OA.
Similarly, equation (13) can be used for the calculation of the driving velocities of joint J3 and joint J4 but not for joint J5. The angular velocities ̇3 and ̇4 can be obtained as:

Design of feedforward torque compensation controller
The mass of the whole exoskeleton manipulator is relatively large. The moment of gravity and the moment of inertia relative to each joint will change greatly with the change of spatial position of the exoskeleton manipulator.
To ensure the effectiveness of speed tracking control, the torque feed-forward compensation control strategy is adopted to accurately compensate for the influence of gravity moment and moment of inertia. The structure block diagram of the control system of single joint J1 is shown in Figure 4. The control current in the current loop is adjusted by a servo pack in AC servo motor controller.
, , , , are the system parameters of each link. ̇1 * is the reference angular velocity input, ̇1 is the actual angular velocity output of J1. and represent the compensation calculation part of inertia moment and gravity moment respectively. B is the total viscous friction factor converted to the motor shaft. n is the reduction ratio.
The total equivalent moment of inertia converted to the motor shaft is: where , , are the moment of inertia of the motor, transmission mechanism and load respectively. The primary goal of the torque feed-forward compensation controller is to design proper and , that is to find suitable ̇1 and . From equation (8) we can obtain: The electrical time constant of the motor is far less than the mechanical time constant of the system. Therefore, the effect of armature inductance can be ignored. From Figure  4, the transfer function of the simplified closed-loop system is: Servo pack is a relatively closed controller. In the reference velocity control mode, the servo pack only opens the voltage interface for velocity control. The parameters , , , , , , of the motor system are unknown while the parameters , of the motor and transmission mechanism are known. The time constant = * / * of the system and * can be measured when no load was applied. * and can be calculated by equation (18). From the differential equation of closed-loop system, we can get: The effect of the change of momentum of inertia can be accurately compensated when ̇= 2̈1 . By submitting equation (18) into equation (8), it can be illustrated as:

Simulation calculation and analysis
The mass and length parameters of the exoskeleton are chosen as: 1 = 300 , 2 = 350 , 1 = 4 , 2 = 3 . When the exoskeleton manipulator rotates in a 3D workspace, the maximum momentum of inertia is 1.6275kg· m 2 calculated by equation (5). According to the reduction ratio of 1/50, the momentum of inertia converted to the motor shaft is 6.51 kg· cm 2 . It has a great impact on the response speed of the motor. Assume that the four joints are driven synchronously by the sine functions: ̇1 = sin( ) , ̇2 = sin(1.3 ) , ̇3 = sin(0.8 ) , ̇4 = sin(1.5 ) , see Figure 5. Since the momentum of inertia of J4 is consistent with the change of position, the changing curve of the momentum of inertia of the three joints along with the change of spatial positions, J1 and J2 and J3, is shown in Figure 6. Figure 7 shows both step response curves with and without the momentum of inertia compensation, where the blue curve indicates no load applied and the red curve indicates with-load applied. After applying the compensation which is addressed in equation (19), the curve is completely coincident with the blue curve under no-load situation.  The simulation results show that the influence of the momentum of inertia on the response time of the system is relatively large with the change of space position. According to Figure 7, the system response is faster (0.02s) after the feed-forward compensation is applied, this result indicates that the control method designed in this study has a better performance in synchronous mirror-image movement. Feed-forward compensation for the momentum of inertia is crucial and effective.

Figure 7
Step response with (blue) and without (red) feed-forward compensation

Experimental verification
The upper limb exoskeleton manipulator [31] is chosen as the experimental verification platform of the mirror driven upper limb rehabilitation robot, as shown in Figure   8.  As shown in Figure 8(a), two gyroscopes were planted on the healthy side of the subject (weight: 75kg, height: 175cm). The gyroscopes located at the elbow joint and wrist joint respectively. In order to reduce the vibration of the exoskeleton during the rehabilitation exercise, the hand exoskeleton is replaced by a grip since the wrist joint does not join the process of rehabilitation. During the experimental test, the healthy upper limb starts moving from the position where the upper arm is vertical and the forearm is slightly horizontal. Make the healthy side conduct an external arc and raise the arm upward, IMU sensors on the paretic side collected the reference angular velocity and drive the motors of exoskeleton tracking the master side of the subject. The end position is shown as Figure 8(b). Recording the output data of gyroscopes gyr1 and gyr2, and import them into the MATLAB program for drawing the plot, as shown in Figure 9 and Figure 10. In the two figures, the dashed lines represent the actual measured data of gyr1 and gyr2, and the solid line represent the filtered data. The red, blue and black lines represent , and respectively. The encoder signals of joints J1, J2, J3 and J4 were collected, and the data of 1 , 2 , 3 , 4 were recorded and calculated, then imported into the MATLAB program to obtain the plot, which is shown in Figure 11. By fusing the two sets of data, the angular velocity control instructions for driving four joint motors are calculated by using the feed-forward compensation control strategy designed in this paper. The position information of the manipulator in space is calculated by the data of 1 , 2 , 3 , 4 through equation (3). The trajectory diagram of the endpoints of the upper arm and forearm is generated by MATLAB, as shown in Figure 12. We have conducted several experiments after the setup and debugging of our system. As long as each module works normally, the exoskeleton manipulator can realize mirror image movement following the healthy upper limb smoothly. The results of the field test and simulations are consistent.

Figure 11
The change curve of the four joint angle 1 , 2 , 3 and 4 .

Figure 12
The trajectory of the endpoints of the upper arm (green) Design of master-slave velocity tracking Upper-limb Exoskeleton System and feed-forward compensation control ·11· and forearm (red)

Conclusions
In this work, we considered a novel master-slave velocity tracking upper-limb exoskeleton system design approach. The angular velocity of the healthy limb is directly converted into the reference tracking angular velocity of four joint motors of the exoskeleton system without precise position information and trajectory setting method. The angular velocity measurement and transmission module designed in this scheme can provide driving control signals for the exoskeleton manipulator and directly drive the paretic upper limb accomplishing rehabilitation exercise according to their own willing. The proposed architecture reduces the weight of wearable exoskeleton for patients and requires less variable as control input. The proposed control scheme guarantees the master\slave exoskeleton could achieve fast respond speed and remain stability while maintaining a good tracking performance. As a prototype product and verification platform of this scheme, it can realize the predetermined function. Simulation results on master\slave rehabilitation exoskeleton system show the efficiency and validity of the suggested scheme in comparison with other method.
In the next step of improvement, we will further study the influence of disturbance of the joint motors since the momentum caused by coupling inertia since centrifugal force and Coriolis force is ignored in our research. Besides, we are looking forward to making a combination of state observer with the present feed-forward compensation control method to solve the problem that caused by unmeasurable state parameters. Finally, the closed-loop system stability needs to be proved by Lyapunov theory.