How To Control A Robot With An Electric Motor

Transcription

1 A Cooperated-Robot Arm Used for Rehabilitation Treatment with Hybrid Impedance Control Method Jingguo Wang and Yangmin Li Department of Electromechanical Engineering, University of Macau, Macao SAR, China ÅÄÙѺÑÓ Abstract. Robot-assisted therapy instruments can help patients to recover their upper limbs or lower limbs function. Nowadays, most two-dof(degree of freedom) robotic arms are applied for upper-limb planar motion in the rehabilitation treatment, while the wrist joint is usually neglected. In this paper, a three-dof planar robotic arm is used as a rehabilitation treatment tool for the survivor after su«ering a stroke or spinal cord injury, aiming to assist them to recover upperlimb function. In order to strengthen the patient s arm, force control in one direction will be desired while the position trajectories in the orthogonal direction should be tracked simultaneously, therefore hybrid impedance control (HIC) is exploited. Redundancy resolution is combined into the control scheme and the constraints of joint limits avoidance(jla) are considered. The simulations are made to study the HIC strategies and the results are discussed accordingly and the tracking e«ectiveness of the proposed method is confirmed. 1 Introduction Patients su«ering from stroke, traumatic brain and spinal cord injuries always need assistance to rehabilitate the a«ected limbs [1]- [6]. By traditional methods with motor rehabilitation therapy in daily clinical routine, the frequent and intensive training guided manually by the physiotherapists is time consuming and requires much labor. Robotic training devices are widely used in rehabilitation therapy for the improvement of the upper-limb of patients. Since human arm is very flexible and the upper limb has several DOFs as shown in Fig. 1, it is very hard to practice the rehabilitation as it really is. Many two-dof robotic arms [1], like MIT-MANUS [3], are applied for upper-limb planar motion in the rehabilitation treatment, but the wrist joint is usually neglected in some works. The patient s wrist should be considered and trained during the basic movement therapy and it will help to recover the whole upper-limb function [4]. In this paper, the shoulder joint is simplified as a single DOF joint and a 3-DOF planar manipulator is formulated to do the initial-step rehabilitation, which will provide the basic analysis for the future design with spatial-dof robotic arm. The movements of three patient s joints(shoulder, elbow and wrist) are driven by mechanical active-assisted robotic arm and then the joints data can be recorded and followed up in the process of relearning motor control later. Corresponding author. H. Liu et al. (Eds.): ICIRA 2010, Part II, LNAI 6425, pp , c Springer-Verlag Berlin Heidelberg 2010

2 452 J. Wang and Y. Li Shoulder Elbow Wrist Fig. 1. The structure of human arm Most of the novel-design rehabilitation robot is configured for safe, stable and compliant operation in close physical contact with humans, which is achieved using impedance control [7]- [12]. Its computer control system modulates the way the robot reacts to mechanical perturbation from a patient or clinician and ensures a gentle compliant behavior [4] [7]. The aim of control is to let the hand track a predefined path in one direction and to impose an assistant or resistant force in the other direction. The hybrid impedance control scheme [2] is a combination of these two fundamental force control strategies, both positions are commanded and a command force trajectory is followed. An augmented hybrid impedance control (AHIC) scheme [12] is proposed for kinematically redundant manipulators combing redundancy resolution to achieve the regulation of the end-e«ector interaction with the environment and satisfy user defined additional constraints. The goal of some robot systems designed is to assist in the rehabilitation of patients with neuromuscular disorders by performing various facilitation movements. In [4], a planar robot was designed with impedance control for guiding patients to make movements along the specified trajectories, which had di«erent stimulatory e«ects on elbow and shoulder joints. A hybrid position»force controller incorporating fuzzy logic was developed to constrain the movement in the desired direction and to maintain a constant force along the moving direction and the planned movements of linear or circular trajectories were considered [1]. A new semi-exoskeleton arm rehabilitation robot having four-dof called ARMin was developed and tested with numerous stroke and SCI patients, and the patient-cooperative control strategy had been presented and evaluated that ARMin can be used to play ball games or to train ADL-related tasks in combination with an audiovisual display [5].

3 A Cooperated-Robot Arm Used for Rehabilitation Treatment with HIC Method 453 In this paper, we utilize a three-dof robotic arm for rehabilitation therapy mainly aiming to assist the a«ected arms of survivors after the stroke and to train their upperlimb function. The robotic devices are connected with the haptic virtual environment. The virtual constraint is seen as a compliant environment and the desired impedance is assumed to be a mass-spring-damper system. The hybrid impedance control is applied not only to track the desired position trajectory but also to follow the force trajectory simultaneously. The other parts of the paper are organized as follows: In section 2 the manipulator s dynamics is introduced. In section 3, hybrid impedance control method for redundant manipulator is carefully discussed. Simulations are performed and their results are demonstrated in section 4 and the conclusion is given out in the last section. 2 Kinematics of the Robotic Arm 2.1 Inverse Kinematics Assume that a task space trajectory is given (x(t) ẋ(t)), and the goal is to find a feasible joint space trajectory (q(t) q(t)) that reproduces the given trajectory. The di«erential kinematics equation establishes a linear mapping between joint space velocities and task space velocities and it has the following form: ẋ J(q) q (1) Due to the non-square Jacobian matrix for redundant manipulator, the basic inverse solution to (1) is obtained by using the pseudoinverse J Ý of the matrix J and the inverse solution can then be written as q J Ý (q)ẋ (2) where the pseudoinverse J Ý can be computed as J Ý J T (JJ T ) 1. For a kinematically redundant manipulator, a nonempty null space exists due to the excess of input space relative to the manipulable space(n m), which is available to set up systematic procedures for an e«ective handling of redundant DOFs. Redundancy will make the robot achieve many advantages such as the avoidance of obstacles, joint limits and the singular configurations, or minimization of impacting forces between the end-e«ector of a manipulator and an environment [13] [11]. 2.2 Desired Acceleration Trajectories in Task Space The joint space dynamics can be transformed into the task space and a set of minimal parameters in the null space by defining the weighted inner product in joint space [8], and a decoupled impedance controller can be applied to control the motion of the ende«ector as well as the internal motion. Based on the HIC idea, the spaces are split into the main task X and additional task Z into position and force controlled subspaces and their equations of motions have similar forms. The accelerations of main tasks (Ẍ) and additional tasks ( Z) have the following forms: Ẍ S x Ẍ d M 1 x [B x (Ẋ S x Ẋ d ) K x S x (X X d ) (I S x )F xd F xe ] (3)

4 454 J. Wang and Y. Li Z S z Z d M 1 z [B z (Ż S z Ż d ) K z S z (Z Z d ) (I S z )F zd F ze ] (4) where S x and S z are diagonal elements of the selection matrix S, S diag(s x S z ) and similarly, M d diag(m x M z ), K d diag(k x K z ), F d diag(f xd F zd )andf e diag(f xe F ze ). 2.3 Redundancy Resolution at the Acceleration Level For compliant motion, dynamic control of redundant manipulators in task space requires the computation of joint accelerations. Hence, redundancy resolution should be performed at the acceleration level. The di«erential kinematics equations are as follows: Ẋ J e (q) q Ẍ J e (q) q J e (q) q (5) where J e (q) is the Jacobian of the end-e«ector. Then the joint acceleration can be solved by the pseudo inverse of the Jacobian matrix Je Ý : q J Ý e (Ẍ J e q) (6) Similar to the pseudo-inverse solution, the following weighted damped least-squares solution [14] can be obtained: q [Je T W e J e Jc T W c J c W v ] 1 [Je T W e (Ẍ J e q) Jc T W c ( Z J c q)] (7) which minimizes the following cost function: L Ëe T W eëe Ëc T W cëc q T W v q (8) where W e, W c and W v are diagonal positive-definite weighting matrices that assign priority between the main, additional and singularity robustness tasks. The velocity errors of the main and additional tasks are Ë e Ẍ d (Ẍ J e q)andë c Z d ( Z J c q). 3 Hybrid Impedance Control for Redundant Robotic Arm 3.1 Dynamics of Robotic Arm in Task Space The dynamics of the open-chain robotic arm can be obtained using energy-based Lagrange method, which is based on the principle of virtual work function. L T V, where the Lagrangian, L, is defined as the di«erence between the kinetic energy T and the potential energy V of the whole system. For manipulator s compliant motion, control tasks are always described in task space. Then in the absence of friction, the dynamics of the robotic arm can be written as: M(X)Ẍ C(X Ẋ)Ẋ G(X) F F e (9) where M(X) is the nonsingular symmetric inertia matrix, C(X Ẋ) is the vector that implicitly includes centrifugal, coriolis and viscous friction, G(X) is the gravity terms, F is the input control force and F e is the interaction force between the robotic arm and the environment. The vector of generalized input force F is related to the vector of input joint torque by the Jacobian matrix J, J T (q)f (10)

5 A Cooperated-Robot Arm Used for Rehabilitation Treatment with HIC Method Hybrid Impedance Control The impedance control proposed in [8] [9] has the following form: M d (ẍ ẍ d ) B d (ẋ ẋ d ) K d (x x d ) F e (11) where M d, B d and K d ¾ R m m are positive-definite matrices representing inertia, damping and sti«ness respectively, which are usually chosen to be diagonal, rendering the system decoupled, and F e is the environmental reaction forces. Impedance control makes the robot behave as a mass-spring-dashpot system, so the force of interaction due to uncertainty on the location of the contact point and environmental structural properties is regulated [2]. However the impedance control is only a position control scheme, with adjustments made to control the contact forces, and it has not any attempt to follow a command force trajectory. Therefore, the hybrid impedance control proposed is defined as follows: M d (ẍ S ẍ d ) B d (ẋ S ẋ d ) K d S (x x d ) (I S )F d F e (12) Here the task space is divided into orthogonal position and force controlled subspaces using the selection matrix S. S is diagonal with ones and zeros on the diagonal while ones on the diagonal relate to position-controlled subspaces and zeros relate to the force-controlled subspaces. F d is the desired force. The desired equation of motion in the position-controlled subspaces is identical to (13). M d ë p B d ė p K d e p F e (13) where e p is the position tracking error and e p X X d. In the force-controlled subspace, the desired impedance is defined by M d ẍ B d ẋ F d F e (14) 3.3 Control Law In the absence of friction and disturbances, the joint-space dynamic equation of rigid manipulator s motion is described by: M(q) q C(q q) q G(q) J T e F e (15) The proposed control scheme is shown in Fig. 2. The inner and outer loop control strategy can be combined to achieve the closed-loop dynamics [12]. The outer loop outputs the acceleration trajectories reflecting the desired impedance in the position-controlled subspaces as pointed in (3) and (4), and the desired force in the force-controlled subspaces of the main and additional tasks as pointed in (14). The inner-loop control is to select an input to the actuators which makes the end-e«ector track the desired trajectories generated by the outer loop. The input torque of the manipulator dynamics is defined as: M(q) q C(q q) q G(q) J T e F e (16)

6 456 J. Wang and Y. Li F xd x d x d x d z d d z d F zd S x S x I S x Outer_loop x F xe B K S x x x -1 M x x -1 M z S z B K S F ze z z z z z S z z I S z z f (q) Redundancy resolution (q) f z x q J (q) (q) J z Computed torque controller Arm C G Inner _ loop q q Fig. 2. The hybrid impedance control scheme 3.4 Null-Space Optimization Many techniques of redundancy resolution are used to search the null space, therefore the search direction and the solution are determined by evaluation of chosen performance criteria. The work [16] presented a detailed discussion about performance criteria from two aspects of constraint-based criteria and operational goal-based criteria, while the former includes joint limit avoidance, velocity limit avoidance, peak torque avoidance, obstacle avoidance and mathematical singularity avoidance and the latter considers dexterity, speed of operation, load carrying capacity, manipulator precision, energy minimizing and other criteria. The most popular method is the gradient projection method (GPM) first introduced in [15] to utilize the redundancy to avoid mechanical joint limits. The particular solution is found using either the pseudoinverse or some other forms of the particular inverse, and the homogeneous solution is then projected onto the null space of the Jacobian matrix. However, there are several problems associated with GPM, including the selection of the appropriate scalar coeæcients that determine the magnitude of the self-motion and oscillations in the joint trajectory, and in many cases, inability to avoid the joint limits in time to avoid disruption. Note that just the joint limit avoidance [11] is considered as local optimization of a performance criterion in this work, and the objective function considered is the distance from mechanical joint limits defined as (q) 1 n n i1 q i q i ( ) 2 (17) q im q im

7 A Cooperated-Robot Arm Used for Rehabilitation Treatment with HIC Method 457 where q im (q im ) denotes the maximum(minimum) limit for q i and q i the middle value of the joint range, and thus redundancy may be exploited to keep the manipulator o«joint limits. 4 Case Study for Robot-Cooperated Therapy Based on HIC Method In order to attempt to provide comprehensive solutions for disabled arm recovery, we utilize a simple planar manipulator to hold one patient s arm closely, which assist the patient just as much as needed to perform a particular movement task while some mechanical quantities such as the position, velocity, and forces will be recorded during the motion. A monitor connected with the whole robotic system provides the motion display for the patients and some games are designed to evaluate the patients performance of upper-limb motion. Here the simulated planar motion aims to train the stroke patients recovering their moving ability as the first step of the rehabilitation work, and a simplified model is to show its application purpose in Fig. 3. A constraint object was placed in the virtual reality (or custom games) and it had been simulated as an elastically compliant system, which can be sensed by haptic-feedback devices. The robotic arm assists the patient to perform some particular movement tasks, which needs whole-joint coordination of shoulder, elbow and wrist. The rehabilitation work requires patients cooperate and interact with the manipulator as a safe, stable and compliant operation, which is achieved using impedance control. Here the hybrid impedance control is considered not only to track the desired position trajectory in one direction (like X) but also to track the desired force trajectory in another direction (like Y). In order to show the e«ectiveness of the proposed control method, the related simulation is carried out in the MATLAB»Simulink. 4.1 Initial Parameters of Simulation In the design of rehabilitation robot, it is necessary to consider that the robotic arm is adaptable to the human limb in terms of segment lengths. In this simulation, the length parameters are defined as l 1 025m, l 2 025m, l 3 01m and the masses of links are m 1 3kg, m 2 2kg, m 3 1kg. Their inertias about the rotating axis have the following values: I z kgm 2, I z kgm 2, I z kgm 2. Another factor to take into account is the range of motion (ROM) of human limb, and then three joints limits of the planar manipulator designed are set as: 0deg q 1 150deg for shoulder joint, 100deg q 2 0deg for elbow joint and 80deg q 3 30deg for wrist joint. The Jacobian matrix of the manipulator is given out as follows: J e (1 1) l 1 s1 l 2 s12 l 3 s123; J e (2 1) l 1 c1 l 2 c12 l 3 c123; J e (1 2) l 2 s12 l 3 s123; J e (2 2) l 2 c12 l 3 c123; J e (1 3) l 3 s123; J e (2 3) l 3 c123;

8 458 J. Wang and Y. Li Wrist DOF l 2 l 3 hand Elbow DOF constraint object F l 1 head Shoulder DOF body Fig DOF planar manipulator imitating human arm The symmetric positive definite inertia matrix M(q) can be expressed by: M(1 1) (10c2 c3 c23); M(1 2) (10c2 2c3 c23); M(1 3) (c3 c23); M(2 2) c3; M(2 3) c3; M(3 3) The matrix of Centrifugal and Coriolis forces C(q) will have the following form: C(1 1) 00125(10d2s2 d3s3 (d2 d3)s23); C(1 2) 00125(10d1s2 d3s3 d1s23); C(1 3) 00125d2s3; C(2 1) 00125(10(d1 d2)s2 d3s3 (d1 d2 d3)s23); C(2 2) 00125d3s3; C(2 3) 00125(d1 d2)s3; C(3 1) 00125(d1 d2 d3)(s3 s23); C(3 2) 00125(d1 d2 d3)s3; C(3 3) 0 where s1 sin(q 1 ); c1 cos(q 1 ); s12 sin(q 1 q 2 ); d1 q 1 and so on. The weighting diagonal elements in the weighting matrices of the redundancy resolution have the values as we 10; wc 100; wv 20. Other parameters for simulation are selected as: M d diag(10); B d diag(150); K d diag(3000).the sti«ness of virtual constraint environment has the value K e 10000Nm.

9 A Cooperated-Robot Arm Used for Rehabilitation Treatment with HIC Method 459 y(m) Rotating angles of three joints (deg) q1 ang q2 ang q3 ang x(m) (a) Sequential moving trajectory time(s) (b) Three joints angles Rotating velocities of three joints(deg/s) q1 vel q2 vel q3 vel Rotating velocities of three joints(deg/s2) q1 acc q2 acc q3 acc time(s) (c) Three joints velocities time(s) (d) Three joints accelerations Fig. 4. Simulation results of hybrid impedance control considering the JLA constraint 4.2 Simulations Results From the equations (3) and (4), when the selection matrix is set to S x diag(0 1), it becomes the hybrid impedance control, which is not only to track the position trajectory in X direction but also to track the force trajectory in Y direction. The robot end-e«ector is moved in the X direction of constraint frame from one point to another point with constant velocity in 4 seconds and the trajectory is calculated by linear interpolation. The desired force trajectory in the Y direction is defined as F d 100N as shown in Fig. 3. An virtual constraint object has been simulated as an elastically compliant system and the interaction force between the robot and the constraint F e can be calculated by the following force equation: F e K e X (18) where X denotes the position error between the constraint and the robot end-e«ector in the constraint frame. The simulation is performed to track both the position trajectory in X direction and the force trajectory F d in Y direction and the constraint of JLA is combined into the redundancy resolution.

10 460 J. Wang and Y. Li 0 20 actual force desired force 40 forces (N) time(s) Fig. 5. Desired force and actual force in the simulation The sequential trajectories of manipulator motion are shown in Fig. 4(a), where the desired position trajectory is tracked well. The rotating angles of all three joints is shown in Fig. 4(b) and it is easily found that all the joints have not exceeded their mechanical limits. The velocities and accelerations of three joints are shown in Fig. 4(c) and Fig. 4(d). It can be observed that the desired force is followed well from Fig. 5 while the error between the actual and desired force become smaller and smaller with time going on. The rotating torques of all three joints are given out in Fig Discussions A robotic arm with higher DOFs will allow a wide variety of movements, which may be more easier to assist the patients arm in space in order to do various rehabilitation training. However, a robotic arm designed with too many DOFs always comes with a lot of problems, such as heavy weight, large size, big inertia and so on. In this work, we only analyze the three-dof planar manipulation, which will provide the basic analysis for our further design. The robotic devices are connected with the haptic virtual environment. The motion designed in the game(or virtual reality) should guide and assist the patient to perform some defined movement tasks, which needs whole-joint coordination of shoulder, elbow

11 A Cooperated-Robot Arm Used for Rehabilitation Treatment with HIC Method 461 Rotating torques of three joints(n.m) q1 tor q2 tor q3 tor time(s) Fig. 6. Rotating torques of three joints in the simulation and wrist. At the same time, some correlated sensory feedback, including visual, audio, and proprioceptive sensory stimuli, should be matched for better rehabilitation. Similar as the structure of human being, the shoulder joint of the rehabilitation robot should be spherical joint, so one or two DOFs will be added to the shoulder joint for more flexible manipulation. 5 Conclusion and Future Work In order to recover the upper-limb of a patient after the stroke, a three-dof planar robotic arm is used to do the rehabilitation treatment, which imitates the actual human arm with shoulder, elbow and wrist joints while the wrist joint is always neglected in previous research works. Then hybrid impedance control method is applied to control both the position and the force trajectories in the constraint frame, where the task space is split into position-controlled subspace and force-controlled subspace by modifying the selection matrix S. The constraints of joint limits avoidance are combined into the redundancy resolution. Corresponding simulations are made with the method of hybrid impedance control, and their results show the proposed control method can track both the position and force trajectory well. In the next step, a real robotic arm will be fabricated and haptic devices will be installed to provide feedback sense. Some games in virtual reality are displayed to interact with the robotic system and then the patient s movement is guided and assisted.

12 462 J. Wang and Y. Li Acknowledgment This work is supported by Macao Science and Technology Development Fund under Grant no. 016»2008»A1 and the Research Committee of University of Macau under grant no. UL016»08-Y2»EME»LYM01»FST. References 1. Ju, M.S., Lin, C.C.K., Lin, D.H., Hwang, I.S., Chen, S.M.: A rehabilitation robot with forceposition hybrid fuzzy controller: hybrid fuzzy control of rehabilitation robot. IEEE Trans. on Neural Systems and Rehabilitation Engineering 13(3), (2005) 2. Liu, G.J., Goldenberg, A.A.: Robust hybrid impedance control of robot manipulators. In: EEE Int. Conf. on Robotics and Automation, California, USA, pp (1991) 3. Formica, D., Zollo, L., Guglielmelli, E.: Torque-dependent compliance control in the joint space of an operational robotic machine for motor therapy. In: IEEE Int. Conf. on Rehabilitation Robotics, Chicago, IL, USA, pp (2005) 4. Krebs, H.I., Hogan, N., Aisen, M.L., Volpe, B.T.: Robot-aided neurorehabilitation. IEEE Trans. Rehab. Eng. 6(1), (1998) 5. Nef, T., Mihelj, M., Riener, R.: Armin: a robot for patient-cooperative arm therapy. Medical and Biological Engineering and Computing 45(9), (2007) 6. Jamwal, P.K., Xie, S., Aw, K.C.: Kinematic design optimization of a parallel ankle rehabilitation robot using modified genetic algorithm. Robotics and Autonomous Systems 57(10), (2009) 7. Hogan, N.: The mechanics of multi-joint posture and movement control. Biological Cybernetics 81, (1999) 8. Oh, Y., Chung, W.K., Youm, Y., Suh, I.H.: Motion»force decomposition of redundant manipulators and its application to hybrid impedance control. In: IEEE Int. Conf. on Robotics and Automation, Leuven, Belgium, pp (1998) 9. Wang, J.-G., Li, Y.M.: Massaging Human Feet by a Redundant Manipulator Equipped with Tactile Sensor. In: IEEE Int. Conf. on Advanced Intelligent Mechatronics, Montreal, Canada, pp (2010) 10. Newman, W.S., Dohring, M.E.: Augmented impedance control: An approach to compliant control of kinematically redundant manipulators. In: IEEE Int. Conf. Robotics and Automation, California, USA, pp (1991) 11. Wang, J.-G., Li, Y.M.: Impedance control of a spatial redundant manipulator used for relaxing muscle fatigue. In: IEEE Int. Conf. on Mechatronics and Automation, Changchun, Jilin, China, pp (2009) 12. Shadpey, F., Patel, R.V., Balafoutis, C., Tessier, C.: Compliant motion control and redundancy resolution for kinematically redundant manipulators. In: American Control Conference, Seattle, WA, pp (1995) 13. de Wit, C.C., Siciliano, B., Bastin, G.: Theory of Robot Control. Springer, London (1996) 14. Seraji, H., Colbaugh, R.: Singularity-robustness and task prioritization in configuration control of redundant robots. In: 29th IEEE Conf. on Decision and Control, Honolulu, HI, USA, pp (1990) 15. Liegeois, A.: Automatic supervisory control of the configuration and behavior of multibody mechanisms. IEEE Trans. Systems, Man, and Cybernetics 7(12), (1977) 16. Kapoor, C., Cetin, M., Tesar, D.: Performance based redundancy resolution with multiple criteria. In: ASME Design Engineering Technical Conference, Atlanta, Georgia, pp. 1 6 (1998)