Proceedings of the 2006 IEEE International Conference on Robotics and Automation Orlando, Florida - May 2006
Development of An Elastic Path Controller B Long1, B Rebsamen1, E Burdet2,1 and CL Teo1 1
Department of Mechanical Engineering, National University of Singapore 2 Department of Biomedical Engineering, Imperial College London, UK E-mail: {long.bo,mpeteocl,brice}@nus.edu.sg,
[email protected]
Abstract— An Elastic Path Controller (EPC) is developed in this paper to add “elasticity” to a path following controller and enable dynamic modification of the guiding path for assistive devices based on path guidance. This permits the users to compensate for changes in the environment such as obstacles or error in position sensing. The EPC is demonstrated both on the Scooter cobot and on the Collaborative Wheelchair Assistant (CWA). Simulation results and psychophysical experiments performed with the Scooter suggest that this novel tool is efficient and can help human operators to adapt to environment changes.
I. I NTRODUCTION Path guidance has been proposed as a strategy to assist manipulation, in the automotive industry [1], in surgery [2], to help people learning handwriting [3], as well as for robotassistive rehabilitation after stroke [4]. A mechatronic device conceived to provide path or surface guidance for planar movements is the Scooter cobot shown in Fig.1A. It is a triangular vehicle moving on a plane with a steerable wheel at each corner. Fig.1B shows the collaborative wheelchair assistant (CWA) we are developping, which uses path guidance to help disabled maneuvering their wheelchairs [5], [6]. The Scooter moves in a three dimensional configuration space, and constrains motion along mechanical guideways or surfaces defined by software. The operator’s intent is inferred from the force/torque applied on the Scooter and detected by a sensor. Two main modes of motion control are used. In free mode (FM), each wheel turns like a caster to align with the
Fig. 1. Mobile platforms used to test the elastic path controller. (A): The Scooter cobot developed at Northwestern University is an assistive device with three degrees of freedom constraining motion along mechanical guideways or surfaces defined by software. (B): The collaborative wheelchair assistant is implemented on a commercial wheelchair with a laptop providing control and the graphical user interface.
0-7803-9505-0/06/$20.00 ©2006 IEEE
493
force exerted by the operator. In guided mode (GM) each wheel is steered by a motor to follow a guiding path coded in software. Our current prototype of the collaborative wheelchair assistant is based on a commercial wheelchair with a laptop providing control and a graphical user interface. The operator’s intent is transferred through the joystick, which angle encodes the velocity signal. The forward angle corresponds to the forward velocity and the side angle the rotational velocity. While it was recently shown that path guidance significantly reduce the (rotational) effort of the operator maneuvering the Scooter [7], [8], constraining the vehicle along a guiding path can also create problems. Hence, obstacles can stand on the path which the device is supposed to track, or a deviation from the desired path may occur due to sensing error. One solution to these problems may consist of switching to free mode in order to deviate from the guiding path and, once the obstacle is passed, switch back to guided mode. This strategy, however, requires additional control from the user, the transition may be unsmooth at both ends, and it is also not clear where to join back the guiding path. To solve these problems, an elastic path controller (EPC) is proposed in this paper, which incorporates “elasticity” into the path constraint controller. With the EPC the operator can let the vehicle deviate from the guiding path for example in order to avoid obstacles, by pushing/pulling perpendicular to the path smoothly after passing the obstacle. The EPC enables users to modify the guiding path when necessary via some interface such as a joystick, so relies on the sensing and inference capabilities of the user, and do not require external sensors and corresponding data processing. This paper describes two versions of an elastic path controller which have been designed for both the Scooter cobot and the collaborative wheelchair (in Section III). This EPC is based on a controller providing asymptotic path tracking, which is defined in Section II. Section IV reports the simulations and experiments with human subjects that have been performed to test this EPC, and to infer how subjects use it. This paper complements [9], in which other aspects of these experiments have been reported. II. PATH F OLLOWING C ONTROLLER We follow the exposition of [10], [11] for the kinematics and corresponding path following controller of the CWA
Fig. 2.
Unicycle kinematics of the Collaborative Wheelchair Assistant. Fig. 3.
Kinematics of the Scooter cobot.
(Section II-A) and the Scooter (Section II-B). A. Collaborative Wheelchair Assistant
by setting
The wheelchair (Fig. 2) has two actuated wheels on a common axis. A reference point is taken at mid-distance of these two wheels. The kinematics of this unicycle-type of vehicle are described relative to a frame consisting of a curvilinear coordinate s along the guiding path, its normal l, and the angle θm relative to a reference Cartesian frame (x, y):
ω= v
s˙
=
l˙ = θ˙m =
v cθ , cθ ≡ cos θ 1 − cc l v sθ , sθ ≡ sin θ ω
(1)
where θ = θm − θc is the error between the vehicle the tangent to the guiding path θc in the orientation θm and (x, y)-frame, v = x˙ 2 + y˙ 2 is the translational speed and cc is the guiding path’s curvature. Equ.(1) depends on time. To develop a path following controller, these equations have to be reparameterized with the t distance traveled by the vehicle along the path, η ≡ |s|. ˙ This yields 0 cθ s = sign v 1 − cc l cθ , tθ ≡ tan θ (2) l = tθ (1 − cc l) sign v 1 − cc l ω |1 − cc l| cθ θ = − cc sign v |v cθ | 1 − cc l d where () ≡ dη . The control objective is to stabilize the output l to zero. A second derivation of l is needed as the control variable ω does not explicitly appear in the expression of l .
l
=
ω 1 + s2 (1 − cc l)2 − cc (1 − cc l) 2 θ − gc l tθ (3) 3 v cθ cθ
This equation is linearized:
l =u
(4) 494
cθ 1 − cc l
u
sin 2θ c2θ + cc (1 + s2θ ) + gc l 1 − cc l 2(1 − cc l)
Using the auxiliary control u ≡ −kpl l − kvl l with kpl > 0, kvl > 0 ,
(5)
it can be shown [10] that, provided that appropriate initial conditions are fulfilled, Equ.(1) has a solution and l converges to 0. B. Scooter Cobot The Scooter cobot is a two-steering type vehicle moving in a three-dimension space configuration space (x, y, θ) (Fig.3). A low-level controller aligns the third wheel to the intersection of the axes of the two steering wheels. Following a similar approach as with the CWA, the Scooter’s position and orientation can first be expressed relative to the curvilinear coordinate s along the guiding path, its normal l and the angle θm relative to a fixed frame (x, y): cθα cθα ≡ cos(θ + α) s = sign v 1 − cc l cθα (6) l = tan(θ + α) (1 − cc l) sign v 1 − cc l 1 − cc l sign(v) − cc sign v cθα θ = σ cθα 1 − cc l α denotes the orientation of the front wheel relative to the line through the two steering wheels, and σ the reciprocal of the length from the leading wheel to the intersection of the normals to the two steering wheels. v is the translational speed, cc the guiding path’s curvature and θc the angle of the tangent to the guiding path relative to frame (x, y). Let θ˜ ≡ θm − θc − θd , where θd is the desired orientation, represent the orientation error. In the case of the Scooter, two control variables α and σ are used to linearize the ˜ Following a equations of two outputs, chosen as l and θ. similar approach as in the CWA case yields l θ˜
=
ul
=
uθ
(7)
.
The elasticity parameter α is used to balance the influence of normal force applied by the user and the attraction from the guiding path. It is computed as
μ 2 2 F⊥ l 1 − +1 I{F⊥ >β} (10) α= 2 F⊥,m lm ν
Fig. 4. Projection of normal input (relative to a local frame of CWA) on the normal to the guiding path used to deviate from this path.
Using the auxiliary control variables ul uθ
= −kpl l − kvl l = −kpθ θ˜ − kvθ θ˜
(8)
with kpl > 0, kvl > 0, kpθ > 0 and kvθ > 0, l and θ˜ asymptotically converge to 0 for suitable initial conditions [10], [11]. III. E LASTIC PATH C ONTROLLER The idea of the Elastic Path Controller is to deform the actual path by allowing the controller to integrate the input from operator, e.g. the force exerted on the Scooter by the operator. We request several features from the EPC: • a larger operator’s input should result in a larger deformation of the guiding path. • the attraction from the desired path should always be felt and it should always be possible to deform the path even when far from it. • the elasticity should not create unwanted oscillations due to non-voluntary deviations.
where 1 > μ > ν > 0 and β > 0. F⊥,m and lm are normalization factors. To insure that the vehicle always can follow the guiding path in elastic mode, we set an upper limit of elastic parameter α = μ. A lower limit of α = ν insures that the user can always deform the trajectory even when the normal distance is large. This is realized through the function [·]μν ≡ min{max{ν, ·}, μ}. Multiplication with I{F⊥ >β} (where I{condition} is the Kronecker function equal to 1 when the condition is fulfilled and 0 otherwise) ensures that no deformation occurs for {|F⊥ | ≤ β}. The closed-loop system function of CWA with elasticity can be calculated as: v cθ s˙ = , cθ ≡ cos θ 1 − cc l (11) l˙ = v sθ , sθ ≡ sin θ c c θ θ θ˙ = v l (gc sθ − (1 − α) kpl cθ ) + 1 − cc l 1 − cc l vcθ + + sθ cc sθ − (1 − α) kvl cθ sign 1 − cc l c2θ − α F⊥ 1 − cc l B. EPC for the Scooter Cobot Following the same approach as in the CWA case, the EPC for the Scooter cobot is realized through: ule uθe
restoring f orce
user s
=
(9)
input
where Kpl > 0, Kvl > 0, and l is the distance from reference point of the CWA on the guiding path. The input F⊥ depends on the joystick angles corresponding to the wheelchair speed. The forward angle (relative to a local frame fixed to the vehicle) corresponds to translatory velocity and the normal angle to rotary velocity. The user input to the EPC is computed as the normal input relative to the current robot direction projected onto the normal to the guiding path (Fig.4). This projection prevents a too large change of orientation relative to the guiding path and limits it to 90o . If the normal to the guiding path would be used directly, the deformation would be larger when the vehicle normal to the path than when it is parallel to it. Therefore, the user may not feel where the guiding path is. 495
(12)
inputf orce
−(1 − αθ )(Kpθ θ˜ + Kvθ θ˜ ) −
restoring torque
To fulfill the above requirements, we modify the controller in Eqn.(5) as follows: αF⊥
−(1 − αl )(Kpl l + Kvl l ) − αl F⊥
restoringf orce
A. EPC for the Collaborative Wheelchair Assistant
u = −(1 − α)(Kpl l + Kvl l ) −
=
αθ τ
input torque
with Kpl > 0, Kvl > 0, Kpθ > 0, Kvθ > 0. The elasticity parameters (αl , αθ ) weigh the influence of normal force and torque applied by the user and measured by the force-torque sensor placed at the driving wheel. They are defined in a similar way as CWA, and the normal force is replaced by the torque τ in αθ . The input from the operator to the EPC depends on the force and torque exerted by the operator on the platform. The ’normal force’ is computed as described in Fig.4. The closed-loop equation of the Scooter cobot EPC is: s˙
=
l˙
=
θ˙
=
α˙
= +
v cθα , cθα ≡ cos(θ + α) 1 − cc l v sθα , sθα ≡ sin(θ + α) cθα v σ − cc 1 − cc l cθα cθα v l (gc sθα − (1 − αl ) kpl cθα ) + 1 − cc l 1 − cc l v cθα + sθα cc sθα − (1 − αl ) kvl cθα sign 1 − cc l
− σ˙
= + + − −
c2θα + cc − v σ (13) 1 − cc l cθα cθα v −(1 − αθ ) kpθ θ˜ + (gc + gd ) + 1 − cc l 1 − cc l cθα l (gc cθα + (1 − αl ) kpl sθα ) + σ 1 − cc l v cθα + sθα cc cθα + (1 − αl ) kvl sθα sign 1 − cc l vcθα cθα (1 − αθ )kvθ σ − (cc + cd ) sign + 1 − cc l 1 − cc l cθα αθ τ 1 − cc l αl F⊥
IV. EXPERIMENTS A. Simulation and Implementation on CWA A simulation environment was realized in MATLAB in order to test the performance of the elastic path controller. Figures 6A,B show the Scooter cobot moving with the EPC when elasticity is not used, from two different starting positions. Corresponding figures 6C,D show how the elasticity enables the user to deform the actual path in order to avoid obstacles. After an obstacle is passed, the controller tracks the guiding path. Further simulations and testing are available in [12]. The EPC was implemented on the CWA. Fig.5A,B illustrates the path tracking ability performance of the EPC when the elasticity is not used, i.e. when the input is below the threshold. These results, similar to results obtained with the path controller of Section II, demonstrate that the EPC works as a path following controller when elasticity is not used. Fig.5C shows how the elasticity enables the operator to avoid a cylinder-shaped obstacle and then return to the guiding path. B. Psychophysical Experiments This section briefly reports experiments performed by 7 healthy subjects maneuvering the Scooter with the EPC. These experiments, which are described in more detail in [9], [12], were designed to investigate whether and how operators could use the EPC. For simplicity, only the elasticity to the normal force was used in these experiments. The parameters were set to μ = 0.9, ν = 0.1 and β = 5N .
Fig. 5. Performance of CWA with the EPC (unit: meter). In(A) the initial position is 1m from the guiding path of distance with 0o orientation; in (B) the orientation is 60o . (C) shows that the operator can easily pass an obstacle using the EPC (unit: meter).
496
Fig. 6. Simulation of Scooter cobot on a straight path in the guided mode (panels A and B) and elastic mode (panels C and D). Two different starting conditions were inspected. The initial orientation of the Scooter is θm (0) = 0o in A and C and θm (0) is 60o in B and D. The initial distance from vehicle to the guiding path is l(0) = 0.7m in all cases. The solid circle represents an obstacle on the desired path. The short arrow attached on the cobot displays the direction of the cobot. The longer arrow shows the direction and magnitude of user’s input by changing arrow’s heading and length. We see that the EPC is not only able to track the desired trajectory well (A and B), but can also bring the vehicle back to the guiding path after passing by the obstacle (C and D).
The first experiment investigated how the operators learned to use the elastic path controller. The subjects were asked to follow a straight path and avoid an obstacle (Fig.7). Using a straight guiding path, the subject was not influenced by the curvature of the guiding path, and he/she could best feel the guidance and elasticity supplied by the elastic path controller. A trial was considered as successful when the subject could avoid the obstacle on the path without hitting it and then come back to the desired straight guiding path. The experiment session was completed when the subject could execute five consecutive trials successfully. The subjects learned to produce suitable force to avoid the obstacle after several failure tries which hit the obstacle
Fig. 7. A first psychophysical experiment tested how users learn to avoid an obstacle placed along a straight line using the elastic path controller.
Fig. 8. Projection of normal input (relative to a local cobot frame) on the normal to the guiding path used to deviate from this guideway.
[9]. Observation of the path and force (Fig.9) of all trials suggests that the first trials were jerky, and motions become smoother with learning. All subjects could learn to pass the obstacle successfully after less than 25 trials and using in mean 17 trials or 20 minutes [12]. During learning, the subjects decreased the high-frequency content of the movement significantly, i.e. they learned to avoid obstacles using smoother movements. Fig.10 presents one path and normal force profiles of one representative subject after learning. However, these results do not prove that users will able to perform well in other guiding paths, nor do they unveil the strategy used by the operator. To infer this, we designed a second experiment with paths unknown to the subjects. The subject was moving on a straight line and instructed to avoid an obstacle, a cylindrical object, at his or her left hand side (Fig.11). After the obstacle, the path diverged to any of four directions. The direction is selected randomly, without any (visual) cues to the subject. Therefore the subject could not know which virtual path will be used in a given trial. Each subject performed twelve trials: three in each of the four directions. In order to infer the strategy used by the operator, we determined, for each subject, the time after which the 12 paths diverge. Fig.12 shows how a divergence time was com-
Fig. 9.
Evolution of normal force of one subject during repeated trials.
497
Fig. 10. The upper panel shows the distance between Scooter and the guiding path and the lower one is the normal force used during one trial of a subject. The figure shows how the subject uses the elastic mode to avoid the obstacle.
puted from the standard deviation. The corresponding point of divergence was taken on the mean trajectory. We computed also, for each trial (of each subject), the corresponding point of dropping force as the last minimum of force (Fig.13). Fig.14 shows, for each of the 4 guiding path directions and for each of the 7 subjects, the difference between the x component of divergence and the mean of the three force dropping points in a direction. These differences appear to be consistent in all directions. An ANOVA confirmed that these differences yields a similar result independent on the direction (the equality is rejected with p > 0.95). Further these differences were significantly larger than 0 with p < 0.002 [9], meaning that the subjects first dropped the force after having passed the obstacle and trusted the EPC, which then led them to the particular trajectory. In summary, the analysis of the force and trajectories indicated that the subjects used a similar strategy independent on the direction of the guiding path [9]. The operators first push or pull against the guiding path with suitable force to avoid the obstacle. After the obstacle is passed, the normal force is released, i.e. the operators trust the path controller, which leads them back to the guiding path. In turn, these results with randomly chosen path demonstrate that the users had learned to work with the elastic path controller independent of the path.
Fig. 11. Environment to infer how the subjects used the elastic path controller.
Fig. 12. Determination of divergence time using the standard deviation of y position (as a function of the time).
V. D ISCUSSION This paper introduced an elastic path controller (EPC) designed for robotic devices. This controller combines the functionalities of path tracking and on-line modification of trajectory, which enables the operator to compensate for modifications of the environment such as a new obstacle or error of position sensing. Mathematical treatment of stability and convergence properties of the EPC are under way [13]. Such an EPC was derived and tested in simulations and implementations, both on the Collaborative Wheelchair Assistant (CWA) and on the Scooter cobot. Psychophysical experiments performed on the Scooter demonstrated that users can learn to modify the guiding paths in a relatively simple way by using the EPC, and suggested that the users may be helped in maneuvering the Scooter by feeling the attraction from the guiding path. However, additional experiments are needed with the Scooter in order to examine how operators can use the orientation elasticity. Psychophysical experiments should also be performed with disabled using the CWA in order to examine whether/how useful the EPC is to the real end users. Efficient use of the EPC may require force feedback to provide feel of the guiding path attraction, i.e. of the distance to this path,
Fig. 13. Force profiles of one typical subject with force dropping times depicted as ‘+’.
498
Fig. 14. Differences between the divergence point and the mean x position corresponding to the dropping time for the 4 different guiding paths. Each bar corresponds to the difference between the mean x position of dropping point over three trials in one direction and the divergence point, for a given subject.
to the wheelchair user. ACKNOWLEDGMENTS The authors thank Eric Faulring, Ed Colgate and Michael Peshkin for their help and advices in the experiments. R EFERENCES [1] MA Peshkin, JE Colgate, W Wannasuphoprasit, CA Moore, RB Gillespie and P Akella (2001), Cobot Architecture, IEEE Transactions on Robotics and Automation, 17(4): 377-90. [2] BL Davies, WJ Lin, RD Hibbert and J Cobb (1997), Active Compliance in Robotic Surgery - the Use of Force Control as a Dynamic Constraint, J of Engineering in Medicine, Proc H of the Institution of Mechanical Engineers, 211(H4): 285-92. [3] E Burdet, CL Teo and HP Lim (2002), A Robotic Teacher for Chinese Ideograms. Haptic Symposium, IEEE International Conference on Virtual Reality (IEEEVR) 335-41 [4] DJ Reinkensmeyer, JL Emken and SC Cramer (2004), Robotics, Motor Learning, and Neurologic Recovery, Annual Review of Biomedical Engineering 6: 497-525. [5] ES Boy, CL Teo and E Burdet (2002), Collaborative Wheelchair Assistant, Proc. IEEE/RSJ International Conference on Robotics and Intelligent Systems(IROS): 344-50. [6] ES Boy, E Burdet, CL Teo and JE Colgate (2002), The Learning Cobot, Proc. ASME International Mechanical Engineering Congress and Exposition. [7] ES Boy, E Burdet, CL Teo and JE Colgate (2003), Motion Guidance Experiment with the Scooter Cobot, Haptic Symposium, IEEE International Conference on Virtual Reality 63-9 [8] ES Boy, E Burdet, CL Teo and JE Colgate (2003), Experimental Evaluation of the Learning Cobot, EuroHaptics. [9] B Long, B Rebsamen, E Burdet, H Yu and CL Teo (2005) Elastic Path Controller for Assistive Devices, 27th Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBS). [10] C. Canudas de Wit, B. Siciliano and G. Bastin, Theory of robot Control, Springer, 1996. [11] A. Micaelli and C. Samson, ”Trajectory tracking for unicycle type and two-steering-wheels mobile robots”, Technical report, INRIA, 1993. [12] B Long (2005), Development of An Elastic Path Controller for Collaborative Robots, Master Thesis, National University of Singapore. [13] J Cao (2005), A dedicated path controller for unicycle-type cobots, Master Thesis, Imperial College London, UK.
